Quick-change type heterogeneous end effector integrated on collaborative robot and control method of quick-change type heterogeneous end effector

Abstract

The invention discloses a quick-change heterogeneous end effector integrated on a collaborative robot and a control method of the quick-change heterogeneous end effector, and relates to the technical field of rehabilitation physiotherapy. By adopting the combination of the six-dimensional force sensor and the stereoscopic vision depth camera, rapid switching of multiple physiotherapy modes is realized, and the accuracy and diversity of the physiotherapy process are improved. The probe design refers to finger pressing, palm pressing, hand pinching and other actions of professional physiotherapy doctors, contact force stability is ensured, the accuracy and comfort of the physiotherapy process are ensured, meanwhile, real-time monitoring is conducted, and physiotherapy parameters are automatically adjusted according to the physical state and requirements of a patient. And by adopting the six-dimensional force sensor and the calibration zero setting technology, the accuracy of force feedback and the signal stability are remarkably improved, and the physical therapy effect is prevented from being influenced by external interference. And rapid replacement and efficient operation of the probe are achieved, the multifunctionality of the collaborative robot is improved, and the service life of the collaborative robot is prolonged. The physical therapy effect is guaranteed, meanwhile, the working intensity of physical therapy doctors is effectively relieved, and wide application prospects and clinical value are achieved.

Description

Technical Field

[0001] The present invention belongs to the field of rehabilitation and physiotherapy technology, and in particular relates to a quick-change heterogeneous end effector integrated in a collaborative robot and a control method thereof. Background Art

[0002] In response to the growing market demand for rehabilitation therapy, existing therapy equipment suffers from common problems such as reliance on manual operation, limited therapy modes, and insufficient precision, which directly impacts treatment effectiveness and patient recovery. Most therapy devices require therapists to manually adjust therapy parameters and struggle to adapt to individual patient needs. Therapists' precision and judgment are crucial factors in the therapy process, but due to the limitations of the equipment, flexibility and precision are limited.

[0003] Existing physical therapy devices typically only support a single therapy mode, making it difficult to quickly switch between treatment modes based on the patient's varying needs. Furthermore, traditional physical therapy devices cannot monitor the patient's physical changes in real time or adjust force feedback during treatment, potentially limiting the effectiveness of treatment. Physical therapists must constantly monitor subtle changes in the body during treatment, and prolonged, high-intensity manual manipulation can easily lead to fatigue, impacting the quality of treatment.

[0004] Currently, the market lacks intelligent devices that can freely switch between multiple therapy modes. Traditional control algorithms, particularly in dynamic interactive scenarios, suffer from rigid parameters and poor environmental adaptability, making them difficult to meet the force control precision requirements of complex human-machine collaboration. Therefore, improving the personalization, flexibility, and precision of therapy and reducing the workload of therapists have become urgent challenges. Summary of the Invention

[0005] In response to the shortcomings of the existing technology, the present invention proposes a quick-change heterogeneous end effector integrated in a collaborative robot and a control method thereof, aiming to solve the need for frequent switching of therapy modes during rehabilitation therapy, and provide a method for rapid switching and optimized control suitable for multiple rehabilitation therapy modes and intelligent therapy equipment, so as to achieve rapid switching of multiple therapy modes, enable the collaborative robot to provide patients with diversified rehabilitation therapy services, and effectively reduce the labor intensity of physiotherapists.

[0006] The technical solution of the present invention is:

[0007] The present invention provides a quick-change heterogeneous end effector integrated into a collaborative robot, comprising a stereoscopic vision depth camera, a six-dimensional force sensor, a quick-change mechanism base, and a physiotherapy probe;

[0008] The six-dimensional force sensor is fixed to the top of the physiotherapy probe and is bolted to the end flange of the collaborative robot. It is used to measure the contact force and contact torque between the physiotherapy probe and the patient's skin during the physiotherapy process and send them to an external computer; the stereoscopic vision depth camera is fixed on one side of the six-dimensional force sensor, maintaining a set distance from the axis of the physiotherapy probe, and is used to obtain planar images and point cloud data of the patient's skin surface and send them to an external computer; the quick-change mechanism base is directly fixed to the bottom of the six-dimensional force sensor by bolts, and a buckle groove is provided on it, which cooperates with the rotating buckle on the physiotherapy probe to quickly fix physiotherapy probes of different modes on the quick-change mechanism base. The physiotherapy probe is used to perform different modes of physiotherapy on the patient.

[0009] Furthermore, the external computer flattens the point cloud data acquired by the stereoscopic vision depth camera, and then superimposes it with the plane image to generate a high-precision two-dimensional plane image integrating spatial depth features, and at the same time generates a three-dimensional skin model of the area to be treated based on the acquired point cloud data, and displays the high-precision two-dimensional plane image integrating spatial depth features and the three-dimensional skin model in real time on the screen of the external computer; provides a human-computer interaction function, and the therapist sets the treatment target based on the high-precision two-dimensional plane image integrating spatial depth features and the three-dimensional skin model of the area to be treated displayed on the screen; the external computer plans the path according to the treatment target, and generates control instructions based on the treatment path, the joint angles of the collaborative robot in real time, the coordinates and normal direction of the center point of the end flange, and the measured contact force and contact torque between the treatment probe and the patient's skin, and sends them to the collaborative robot and the treatment probe; the treatment target includes the treatment range and the treatment plan, and the treatment plan includes the sequence of various treatment modes during the treatment process, as well as the working frequency and contact force size under each treatment mode.

[0010] Furthermore, the physiotherapy probe includes a hand-pinching heterogeneous probe, a finger-pressure heterogeneous probe and a palm-pressing heterogeneous probe, which respectively correspond to the hand-pinching mode, finger-pressure mode and palm-pressing mode in the physiotherapy process; a rotating buckle is set on the top of the hand-pinching heterogeneous probe, and two groups of symmetrically distributed bionic four-finger clamps are provided at the bottom. The hand-pinching heterogeneous probe has a built-in screw stepper motor and a single-chip microcomputer. The screw stepper motor drives the two groups of symmetrically distributed bionic four-finger clamps to open and close to complete the hand-pinching action; the single-chip microcomputer is used to receive control instructions from an external computer and dynamically adjust the opening and closing, massage strength and working frequency of the hand-pinching heterogeneous probe; the tops of the finger-pressure heterogeneous probe and the palm-pressing heterogeneous probe are rotating buckles of the same size, and the bottom of the finger-pressure heterogeneous probe is a thumb-shaped structure; the palm-pressing heterogeneous probe is a semi-ellipsoidal curved surface structure.

[0011] Furthermore, the six-dimensional force sensor and the external computer use USB serial communication to send the measured contact force and torque between the physiotherapy probe and the patient's skin to the external computer in real time;

[0012] The built-in single-chip microcomputer of the hand-pinch heterogeneous probe is connected to the port of an external computer via USB Type-C to receive control instructions.

[0013] Furthermore, when the external computer detects that the contact force and torque between the therapy probe and the patient's skin are greater than the set threshold, the therapy process is stopped immediately; a manual operation function is provided on the screen of the external computer, including manual adjustment of the therapy mode and the size of the contact force, and the priority of the manual operation is higher than the control instructions generated by the external computer. At the same time, an emergency stop button is set on the screen to stop the therapy process at any time.

[0014] The present invention also provides a control method for a quick-change heterogeneous end effector integrated in a collaborative robot, which is implemented based on the quick-change heterogeneous end effector integrated in the collaborative robot and includes the following steps:

[0015] Using stereo vision depth cameras to identify therapy targets;

[0016] Calibrate and zero the six-dimensional force sensor;

[0017] Generate a therapy pathway based on the set therapy goals;

[0018] Design a real-time control strategy for the collaborative robot's posture, which is used to generate joint instructions for the collaborative robot based on the therapy path, the joint angles of the collaborative robot, the coordinates and normal direction of the end flange center point, and the measured contact force and contact torque between the therapy probe and the patient's skin;

[0019] Design therapy probe switching strategies;

[0020] According to the therapy goals and the generated therapy path, the collaborative robot and the quick-change heterogeneous end effector integrated in the collaborative robot are controlled to perform therapy. During the therapy, the contact force and torque between the quick-change heterogeneous end effector integrated in the collaborative robot and the patient's skin are obtained in real time using the zero-calibrated six-dimensional force sensor. The collaborative robot's real-time posture control strategy is used to generate joint instructions for the collaborative robot, dynamically adjust the posture of the collaborative robot, and replace the therapy probe according to the therapy probe switching strategy during the therapy.

[0021] Furthermore, the physical therapy path is generated according to the set physical therapy goal, specifically:

[0022] For the acupressure mode, the generated therapy path is a number of discrete acupuncture points, and also includes the movement speed of the acupressure heterogeneous probe, the dwell time at each discrete acupuncture point, and the pressing depth;

[0023] For the palm-press mode, the generated therapy path is a continuous trajectory, which also includes the tilt angle, movement speed and expected contact force of the palm-press heterogeneous probe;

[0024] For the hand-kneading mode, the generated therapy path is a continuous trajectory, which cycles in a contact-hover-recontact pattern with the skin at a set speed, with intermittent kneading actions.

[0025] Furthermore, the real-time control strategy for the collaborative robot's posture is specifically designed to include:

[0026] S1: Construct joint dynamics model;

[0027]

[0028] Wherein, j represents the joint number of the collaborative robot, i represents the joint number of the collaborative robot different from j, k represents the number of joints of the collaborative robot, and n represents the number of joints other than the j-th joint; is the state vector of the collaborative robot; x j represents the angle of the j-th joint of the collaborative robot; represents the angular velocity of the jth joint of the collaborative robot; represents the angular acceleration of the jth joint of the collaborative robot; f j (x) is the known nonlinear dynamics; Δf j (x) is the model uncertainty; b ji (x) is the control gain matrix; Δb ji (x) is the gain perturbation; u i is the control input; d j (t) is the external disturbance; t represents the time that changes with the treatment process;

[0029] S2: Design an SPO active disturbance rejection observer based on the joint dynamics model; the SPO active disturbance rejection observer is used to reconstruct the total disturbance in real time, obtain the disturbance estimate, and then realize disturbance compensation;

[0030] The dynamic equation of the active disturbance rejection observer is:

[0031]

[0032] in, is the estimated position of the end of the therapy probe; is the estimated velocity of the therapy probe tip; is the estimated value of the disturbance at the end of the therapy probe; is the estimated rate of change of the position of the distal end of the therapy probe; is the estimated acceleration of the physiotherapy probe tip; is the disturbance change rate of the end of the therapy probe; α1j , α 2j is the linearization gain; k 1j and k 2j is the error feedback gain; is the position estimation error; is the speed estimation error; is the estimated value of the total disturbance of the quick-change heterogeneous end effector; sat(·) is the boundary layer saturation function, which is defined as:

[0033]

[0034] Among them, ε 0j is the boundary layer thickness; the sign(·) function is a piecewise function;

[0035] The final disturbance estimate of the ADRO output is The error feedback gain of the ADRO must satisfy the stability condition:

[0036]

[0037] Among them, λ dj is the dominant pole parameter.

[0038] S3: Combine the ADRO designed in S2 with a PID controller to construct a dual-closed-loop PIDSPO controller, which is then used to generate joint commands for the collaborative robot. The dual-closed-loop PIDSPO controller includes a feedforward channel and a feedback channel. The feedforward channel actively cancels disturbances using the disturbance estimate output by the ADRO, significantly reducing the regulation burden of the feedback channel. The feedback channel uses a classic PID controller to eliminate residual disturbances and modeling errors.

[0039] The double closed-loop control law formula of the double closed-loop PIDSPO controller is:

[0040]

[0041] Among them, F d is the desired total contact force at the end of the therapy probe; J j is the Jacobian matrix corresponding to the three-dimensional coordinates of the collaborative robot and the joint space; τ is the time variable for integration and differentiation operations; k pj 、k ij 、k dj is the PID gain; m j is the equivalent inertia parameter; B j is the linear damping coefficient; e j is the tracking error.

[0042] Furthermore, the dual closed-loop PIDSPO controller is used to generate joint instructions for the collaborative robot as follows:

[0043] S3.1: Output disturbance estimates using an active disturbance rejection observer Combined with the Jacobian matrix J(x), a dynamic estimation model of the end contact force is constructed, and the estimated end contact force is obtained using the dynamic estimation model of the end contact force;

[0044] First, the kinetic parameter correction value ΔJ is calibrated online p , modify the Jacobian matrix J(x):

[0045]

[0046] Among them, J(x) is the original Jacobian matrix, J p (x) is the generalized Jacobian matrix after parameter correction; ΔJ p is the correction value of dynamic parameters; m p is the quality of the therapy probe; I p is the inertia tensor of the therapy probe; r p is the center of mass position of the therapy probe; ω p is the angular velocity of the distal end of the therapy probe; is the angular acceleration of the end of the therapy probe; g = 9.8 m / s 2 is the acceleration due to gravity;

[0047] The dynamic estimation model of the end contact force is:

[0048]

[0049] in, is the estimated end contact force; is the disturbance vector;

[0050] S3.2: Calculate the contact force error based on the expected total contact force of the end of the therapy probe and the estimated end contact force, convert the contact force error into a posture correction value using a PID regulator, and implement outer loop smooth adjustment of the posture correction value;

[0051] The contact force error is:

[0052] The PID regulator formula is:

[0053]

[0054] Where Δx is the steady-state deviation of the collaborative robot’s end position; K fp , K fi , K fd are proportional, integral, and differential adjustment parameters;

[0055] S3.3: Use inverse kinematics to convert pose corrections into joint commands for the collaborative robot.

[0056] The corrected Cartesian space trajectory x′ is solved by inverse kinematics d Convert to joint instructions:

[0057] θ′ d =IK(x′ d )+Δθ comp (16)

[0058] Where x′ d =x d +Δx is the corrected Cartesian space trajectory, x d is the desired end pose; IK(·) is the inverse kinematics solution; is the compliance compensation term, K c is a smooth gain, and θ′ is tracked by the PIDSPO controller d , forming a double closed-loop control.

[0059] Furthermore, the physical therapy probe switching strategy is specifically as follows:

[0060] The collaborative robot first automatically moves the quick-change heterogeneous end effector integrated in the collaborative robot to the quick-change operation area, which is located in an area without any risk of contact with the patient. It then rotates to unlock the old therapy probe and replaces it with a new one. After the replacement is completed, click the confirmation command on the external computer, and then use the collaborative robot to move the new therapy probe to the set position in the therapy path, seamlessly connecting the subsequent therapy process.

[0061] Compared with the prior art, the present invention has the following beneficial effects:

[0062] This invention provides a quick-change heterogeneous end effector integrated into a collaborative robot and its control method. This system utilizes a combination of a six-axis force sensor and a stereoscopic depth camera to enable rapid switching between multiple therapy modes, improving the accuracy and diversity of the therapy process. Compared to traditional manual operation, this invention significantly reduces the workload of therapists and improves therapy efficiency and comfort through automation and data fusion technology. The probe design draws inspiration from professional therapists' finger pressure, palm pressing, and hand pinching techniques to ensure stable contact force, ensuring the accuracy and comfort of the therapy process. The quick-change heterogeneous end effector system can monitor the patient's physical condition and needs in real time and automatically adjust therapy parameters based on the patient's needs. This intelligent and flexible design significantly enhances therapy effectiveness and reduces human error, promoting the development of rehabilitation therapy towards greater efficiency, safety, and personalization. The use of a six-axis force sensor and calibration and zeroing technology significantly improves the accuracy of force feedback and signal stability, preventing external interference from affecting therapy effectiveness. This innovative design also enables rapid probe replacement and efficient operation, enhancing the versatility and service life of the collaborative robot. The present invention effectively reduces the workload of physiotherapists while ensuring the physiotherapy effect, and has broad application prospects and clinical value. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 Schematic diagram of the main structure of a quick-change heterogeneous end effector integrated into a collaborative robot in an embodiment of the present invention;

[0064] Figure 2 Schematic diagram of an exploded view of a quick-change mechanism of a quick-change heterogeneous end effector integrated into a collaborative robot according to an embodiment of the present invention;

[0065] Figure 3 Schematic cross-sectional view of a quick-change heterogeneous end effector integrated into a collaborative robot according to an embodiment of the present invention;

[0066] Figure 4 This is a flow chart of a control method for a quick-change heterogeneous end effector integrated into a collaborative robot according to an embodiment of the present invention;

[0067] Figure 5 This is a block diagram of the control strategy of a quick-change heterogeneous end effector integrated into a collaborative robot in an embodiment of the present invention;

[0068] Figure 6 This is a dual closed-loop control block diagram of a quick-change heterogeneous end effector integrated into a collaborative robot in an embodiment of the present invention;

[0069] Among them, 1-stereoscopic vision depth camera; 2-six-dimensional force sensor; 3-hand-pinch heterogeneous probe; 4-quick-change mechanism base; 5-finger-pressure heterogeneous probe; 6-palm-pressing heterogeneous probe. DETAILED DESCRIPTION

[0070] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0071] This embodiment provides a quick-change heterogeneous end effector integrated into a collaborative robot, such as Figure 1-Figure 3 As shown, the quick-change heterogeneous end effector integrated in the collaborative robot includes a stereoscopic vision depth camera 1, a six-dimensional force sensor 2, a quick-change mechanism base 4 and a physiotherapy probe;

[0072] The six-dimensional force sensor 2 is fixed to the top of the physiotherapy probe and is fixed to the end flange of the collaborative robot in a bolted manner. It is used to measure the contact force and contact torque between the physiotherapy probe and the patient's skin during the physiotherapy process and send them to an external computer, effectively improving the control accuracy of the constant contact force; the stereoscopic vision depth camera 1 is fixed to the left side of the six-dimensional force sensor 2, with a distance of 120 mm from the axis of the physiotherapy probe. This position is least obstructed by the structure and is used to shoot the physiotherapy working area in real time, obtain the plane image and point cloud data of the patient's skin surface and send them to an external computer; the quick-change mechanism base 4 is directly fixed to the bottom of the six-dimensional force sensor 2 by bolts, and a buckle groove is provided on it, which cooperates with the rotating buckle on the physiotherapy probe to quickly fix physiotherapy probes of different modes on the quick-change mechanism base 4, meeting the quick-change requirements of multiple physiotherapy modes. The physiotherapy probe is used to perform physiotherapy of different modes on the patient;

[0073] The external computer flattens the point cloud data acquired by the stereoscopic vision depth camera 1, and then superimposes it with the plane image to generate a high-precision two-dimensional plane image integrating spatial depth features, and at the same time generates a three-dimensional skin model of the area to be treated based on the acquired point cloud data, and displays the high-precision two-dimensional plane image integrating spatial depth features and the three-dimensional skin model in real time on the screen of the external computer; provides a human-computer interaction function, and the user sets a treatment target based on the high-precision two-dimensional plane image integrating spatial depth features and the three-dimensional skin model of the area to be treated displayed on the screen; performs path planning based on the treatment target, and generates control instructions based on the path planning results, the joint angles of the collaborative robot in real time, the coordinates and normal direction of the center point of the end flange, and the measured contact force and contact torque between the treatment probe and the patient's skin, and sends them to the collaborative robot and the treatment probe for treatment; the treatment target includes a treatment range and a treatment plan, and the treatment plan includes the sequence of various treatment modes during the treatment process, as well as the working frequency and contact force size of each treatment mode;

[0074] The physiotherapy probe includes a hand-pinching heterogeneous probe 3, a finger-pressing heterogeneous probe 5 and a palm-pressing heterogeneous probe 6, which respectively correspond to the hand-pinching mode, finger-pressing mode and palm-pressing mode during the physiotherapy process; the hand-pinching heterogeneous probe 3 is provided with a rotating buckle on the top and two groups of symmetrically distributed bionic four-finger grippers on the bottom, with a width of 72 mm. The hand-pinching heterogeneous probe 3 has a built-in screw stepper motor and an Arduino UNO single-chip microcomputer. The screw stepper motor drives the two groups of symmetrically distributed bionic four-finger grippers to open and close to complete the hand-pinching action; the Arduino The UNO single-chip microcomputer is used to receive control commands from an external computer and dynamically adjust the opening and closing, massage intensity, and operating frequency of the hand-kneading heterogeneous probe 3. The finger-pressure heterogeneous probe 5 and the palm-pressing heterogeneous probe 6 are both integrated 3D-printed, with the tops having rotating buckles of the same size. The bottom of the finger-pressure heterogeneous probe 5 has a thumb-shaped structure that is 1:1 simulated of a normal adult male thumb. The palm-pressing heterogeneous probe 6 has a semi-ellipsoidal curved surface structure that approximates the palm of an adult male (long axis 84mm, short axis 52mm, and dynamically changing curvature radius), which can realize physical therapy techniques such as sliding.

[0075] The six-dimensional force sensor 2 uses USB serial communication with the external computer to send the measured contact force and torque between the physiotherapy probe and the patient's skin to the external computer in real time at a baud rate of 115200bps;

[0076] The Arduino UNO microcontroller built into the hand-pinch heterogeneous probe 3 is connected to the COM4 port of an external computer via USB Type-C to receive control instructions such as pressing force, massage frequency, and start / stop signals. The collaborative robot establishes a two-way connection with the external computer via an Ethernet cable. The collaborative robot feeds back the angles of each joint, the coordinates of the center point of the end flange, and the normal direction to the external computer in real time. The external computer sends control instructions in real time to drive the collaborative robot to move, thereby driving the quick-change heterogeneous end effector integrated in the collaborative robot to complete the physical therapy process.

[0077] During any physical therapy session, the physical therapist has the highest priority to control the treatment process at any time, including adjusting the treatment mode and contact force to ensure the safety of the treatment process. In order to ensure the safety of the treatment process, necessary safety redundancy design has been carried out. First, a manual operation function is provided on the external computer to ensure that the treatment process can be fully controlled in an emergency. The physician can stop the treatment process at any time using the emergency stop button on the external computer to effectively deal with possible unexpected situations, thereby ensuring the safety of the treatment area. At the same time, the contact force and torque thresholds between the treatment probe and the patient's skin are set to prevent damage caused by excessive contact force or probe movement. In addition, the quick-change heterogeneous end effector system also has a contingency plan for dealing with unexpected situations to ensure sufficient safety during the treatment process.

[0078]

[0079] Among them, F sum is the total shear force; F x ,F y are the tangential forces in the X-axis and Y-axis directions respectively; F thresh is the safety threshold of the lateral force, usually set to 5N; when F sum >F thresh When the vehicle is moving, it will trigger an emergency stop or reduce the lateral speed to avoid skin abrasions;

[0080] like Figure 4-Figure 6 Figure 2 demonstrates a control method for a quick-change heterogeneous end-effector integrated into a collaborative robot. This control method is based on a disturbance-compensated dual-closed-loop tracking control architecture. It utilizes an outer-loop impedance model for compliant contact force control, while an inner-loop composite controller provides high-precision trajectory tracking control. This control method integrates the therapy goals set by the therapist to plan a comprehensive therapy path. The force / torque data fed back by the six-dimensional force sensor is calibrated and filtered. After complex calculations, accurate control instructions are issued in real time, ensuring safe and effective therapy services.

[0081] This embodiment provides a control method for a quick-change heterogeneous end effector integrated into a collaborative robot, comprising the following steps:

[0082] Step 1: Determine the therapy target using a stereo vision depth camera 1;

[0083] Specifically, a quick-change heterogeneous end effector integrated in the collaborative robot is installed on the end flange of the collaborative robot, and the physical therapist uses an external computer to control the movement of the collaborative robot so that the stereoscopic vision depth camera 1 carried by the quick-change heterogeneous end effector maintains a distance of about 30 cm from the patient's skin to perform a complete scan of the area to be treated, so as to obtain a plane image and point cloud data of the entire area to be treated; the point cloud data obtained by the stereoscopic vision depth camera 1 is flattened, and then superimposed with the plane image to generate a high-precision two-dimensional plane image integrating spatial depth features, and at the same time, a three-dimensional skin model of the area to be treated is generated based on the collected point cloud data, and then the relative spatial coordinates of the quick-change heterogeneous end effector integrated in the collaborative robot and the area to be treated are obtained, and the high-precision two-dimensional plane image integrating spatial depth features and the three-dimensional skin model are displayed in real time on the screen of the external computer;

[0084] Physicians analyze the skin surface morphology presented in high-precision two-dimensional images fused with spatial depth features, combined with a three-dimensional skin model of the area to be treated. They then preliminarily assess the spatial distribution of the lesion and the local skin condition. Combining clinical experience, they provide an accurate clinical diagnosis and clarify the treatment goals.

[0085] Physicians mark the boundary curve of the treatment area through interactive click operations on a high-precision two-dimensional plane image integrated with spatial depth features displayed on an external computer. They clarify the spatial mapping relationship between the treatment area and the patient's three-dimensional skin model and provide a personalized treatment plan that matches the patient's physiological characteristics.

[0086] Step 2: Calibrate and zero the six-dimensional force sensor 2;

[0087] During the treatment process, the six-dimensional force sensor 2 provides real-time feedback on the contact force and torque (F) between the quick-change heterogeneous end effector integrated in the collaborative robot and the patient's skin. x ,F y ,F z ,M x ,M y ,M z ),in:

[0088] F z Normal force directly reflects the pressing depth and strength. It is the core parameter of the quick-change heterogeneous end effector. During the physical therapy process, the normal force F must be maintained. z Stay within the safety threshold (e.g., 5-30N) to avoid soft tissue damage;

[0089] F x ,F y are the tangential forces in the X-axis and Y-axis directions, representing the lateral sliding friction or shear force between the therapy probe and the skin. F x ,F y The range may vary slightly, such as suppressing lateral sliding (F x ,F y ≈0N); massage technique (2~5N);

[0090] M x ,M y They are the torque around the X axis and the torque around the Y axis, reflecting the tilted contact of the therapy probe or the uneven distribution of local pressure (such as excessive pressure on one side). x ,M y If the value is too large, the collaborative robot posture should be adjusted to make the torque approach zero (M x ,M y →0), ensuring that the therapy probe contacts evenly;

[0091] M z The torque around the Z axis indicates the rotational resistance of the physiotherapy probe (such as the twisting action during fascial release). The rotation angle should be controlled or the torque should be limited according to the needs of physiotherapy (such as |M z |<0.5);

[0092] Before using a single six-dimensional force sensor 2, perform calibration and zeroing according to its user manual. In this embodiment, a stable debugging environment with constant temperature and no vibration is first created to ensure that the six-dimensional force sensor 2 is rigidly installed and has no external load. After preheating for 30 minutes to stabilize the circuit, the six-dimensional force sensor 2 is collected from each channel (F) of the six-dimensional force sensor 2 in the no-load state of the quick-change heterogeneous end effector. x ,F y ,F z ,M x ,M y ,M z ) is used as the zero offset, and then the zero adjustment is performed according to the zero offset compensation formula;

[0093] The zero offset compensation formula is:

[0094] F corrected =F raw -F offset (2)

[0095] Among them, F corrected is the compensated force and torque vector, that is, the actual measurement value after eliminating zero drift; F raw F is the force and torque vector originally output by the six-dimensional force sensor, usually in N and N·m; offest =[F x ,F y ,F z ,M x ,M y ,M z ] T is the zero-point offset vector, including the steady-state output mean of each channel when there is no load;

[0096] Step 3: Generate a therapy pathway based on the set therapy goals;

[0097] This therapy path is centered around the spatial motion trajectory of the quick-change heterogeneous end effector integrated into the collaborative robot, and is differentiated by combining the functional characteristics of three probes: finger pressure, palm pressure, and hand pinching.

[0098] For the acupressure mode, the generated therapy path adopts a multi-point continuous contact strategy based on the meridian direction. Discrete acupuncture points with a spacing of 15-20mm are planned in high-density acupuncture points such as the shoulder, neck, and soles of the feet. The acupressure-type heterogeneous probe 5 performs single-point vertical pressing operations in sequence at a speed of 0.05m / s, maintaining a dwell time of 0.8-1.2 seconds at each point, achieving precise pressing stimulation at a depth of 3-5mm.

[0099] For the palm-pressing mode, the treatment path is designed as an S-shaped continuous trajectory distributed along large muscle groups such as the back and thighs. The inclination angle of the palm-pressing heterogeneous probe 6 is dynamically adjusted based on the curvature data in the patient's three-dimensional skin model. A reciprocating sliding motion is performed along the muscle texture at a speed of 0.12m / s. At the same time, a gradual desired contact force is set (starting force 3N → maximum force 8N) to achieve progressive massage that fits the muscle texture.

[0100] In manual pinching mode, the treatment path is a wavy, continuous trajectory that covers soft tissue areas such as the trapezius muscle. The manual pinching isomorphic probe 3 performs a "contact-hover-recontact" cycle at a speed of 0.08 m / s (single contact duration 1.5-1.8 seconds, hovering height 5-15 mm). This kneading action, with intermittent pressure pulses (frequency 0.2-0.5 Hz), enhances stimulation of the deep fascia.

[0101] Step 4: Design a real-time control strategy for the collaborative robot's posture. The real-time control strategy for the collaborative robot's posture is used to generate joint instructions for the collaborative robot based on the physical therapy path, the joint angles of each joint fed back by the collaborative robot in real time, the coordinates and normal direction of the end flange center point, and the measured contact force and contact torque between the physical therapy probe and the patient's skin;

[0102] Step 4.1: Construct joint dynamics model;

[0103] During the physical therapy process, the collaborative robot equipped with a quick-change heterogeneous end effector integrated into the collaborative robot will be subject to multiple nonlinear interferences such as joint friction, load changes, and probe inertia differences. These factors directly affect the control accuracy of the contact force. To achieve high-precision control of the physical therapy collaborative robot, a complete dynamic model that includes nonlinear dynamics and disturbances must be established. This invention introduces model uncertainty terms to describe unmodeled dynamics and expands it to multi-joint systems based on a single degree of freedom. The joint dynamics model is expressed as:

[0104]

[0105] Wherein, j represents the joint number of the collaborative robot, i represents the joint number of the collaborative robot different from j, k represents the number of joints of the collaborative robot, and n represents the number of joints other than the j-th joint. In this embodiment, the collaborative robot has a total of 7 joints, so j = 1, ..., 7; is the state vector of the collaborative robot; x j represents the angle of the j-th joint of the collaborative robot; represents the angular velocity of the jth joint of the collaborative robot; represents the angular acceleration of the jth joint of the collaborative robot; f j (x) is the known nonlinear dynamics (including Coriolis force, centrifugal force and gravity terms); Δfj (x) is the model uncertainty; b ji (x) is the control gain matrix; Δb ji (x) is the gain perturbation; u i is the control input; d j (t) is the external disturbance; t represents the time that changes with the treatment process;

[0106] Step 4.2: Designing an active disturbance rejection observer (SPO) based on the joint dynamics model to address disturbances during the treatment process. The SPO is used to reconstruct the total disturbance in real time, obtain a disturbance estimate, and then implement disturbance compensation.

[0107] The total disturbance ψ experienced by the quick-change heterogeneous end effector integrated into the collaborative robot during physical therapy j (x, t) is defined as the deviation between the actual dynamic behavior of the quick-change heterogeneous end-effector system and the ideal model, which includes the nonlinear characteristics of the collaborative robot itself, the changes in inertial parameters caused by the quick change of the physiotherapy probe, and the external force caused by the patient's body movement.

[0108] Total disturbance ψ j The mathematical expression of (x, t) is:

[0109]

[0110] The boundedness of the disturbance is an important prerequisite for the stability of the controller. Assuming that the amplitude of each disturbance component changes with time but does not exceed the known upper bound function, the disturbance satisfies the boundedness condition:

[0111]

[0112] Among them, Γ j (x, t) is the perturbation upper bound function; F j (x), Φ ji and D j (t) is a known upper bound function, and F j (x)>|Δf j (x)|、Φ ji >|Δb ji (x)|、D j (t)>|d j (t)|;

[0113] Then, by introducing the control variable α 3j Linearize the fast joint dynamics model:

[0114]

[0115] in, is the theoretical optimal control input variable after linearization;

[0116] The linearization of the quick-change heterogeneous end-effector system separates the ideal model and the disturbance term, which can clearly define the boundary between known dynamic characteristics and unknown interference, laying the foundation for subsequent design of disturbance observation and compensation.

[0117] The self-disturbance rejection observer (SPO) is based on the state error feedback mechanism. By constructing a virtual extended state, the actual disturbance and model uncertainty are unified as the dynamic changes of the extended state, and the total disturbance ψ is reconstructed in real time. j (x, t), the dynamic equation of the active disturbance rejection observer is:

[0118]

[0119] in, is the estimated position of the end of the therapy probe; is the estimated velocity of the therapy probe tip; is the estimated value of the disturbance at the end of the therapy probe, which is used to compensate for external disturbances; is the estimated rate of change of the position of the distal end of the therapy probe; is the estimated acceleration of the physiotherapy probe tip; is the disturbance change rate of the end of the therapy probe; α 1j , α 2j is the linearization gain, which is used to balance the coupling effect between the quick-change heterogeneous end-effector system and the dynamic disturbance; k 1j and k 2j is the error feedback gain, which determines the convergence speed of the estimated value; is the position estimation error; is the speed estimation error; is the estimated value of the total disturbance of the quick-change heterogeneous end effector; sat(·) is the boundary layer saturation function, which is used to limit the amplitude of the error signal and avoid high-frequency oscillation. It is defined as:

[0120]

[0121] Among them, ε 0j is the boundary layer thickness, which determines the size of the error fluctuation range; the sign(·) function is a piecewise function used to determine the positive and negative signs of the input value.

[0122] The final disturbance estimate of the ADRO output is The error feedback gain of the ADRO must satisfy the stability condition:

[0123]

[0124] Among them, λ dj is the dominant pole parameter, which determines the dynamic characteristics of the ADRO, λdj The larger the value, the faster the ADRO responds. dj With ε 0j , which can strike a balance between estimation speed and noise resistance;

[0125] Step 4.3: Combine the active disturbance rejection observer (SPO) designed in Step 4.2 with a PID controller to construct a dual-closed-loop PIDSPO controller, and use the dual-closed-loop PIDSPO controller to generate joint commands for the collaborative robot. The dual-closed-loop PIDSPO controller includes a feedforward channel and a feedback channel. The feedforward channel actively cancels disturbances using the disturbance estimate output by the active disturbance rejection observer (SPO), significantly reducing the regulation burden of the feedback channel. The feedback channel uses a classic PID controller to eliminate residual disturbances and modeling errors.

[0126] The double closed-loop control law formula of the double closed-loop PIDSPO controller is:

[0127]

[0128] Among them, F d is the desired total contact force at the end of the therapy probe; J j is the Jacobian matrix corresponding to the three-dimensional coordinates of the collaborative robot and the joint space; τ is the time variable for integration and differentiation operations; k pj 、k ij 、k dj is the PID gain; m j is the equivalent inertia parameter (matching the mass of the therapy probe); B j is the linear damping coefficient; e j is the tracking error;

[0129] By using the disturbance estimate output by the active disturbance rejection observer (SPO) Feedforward compensation, the closed-loop dynamics of the quick-change heterogeneous end effector system is simplified to:

[0130]

[0131] in, is the first-order derivative of the tracking error; is the second-order derivative of the tracking error; is the residual disturbance;

[0132] Select the gain to meet the conditions: k pj >k dj / 2, and through the Lyapunov function Stability analysis was performed to rigorously prove the asymptotic convergence of the double-loop PIDSPO controller, providing theoretical security for subsequent design.

[0133] Step 4.3.1: Use the Active Disturbance Rejection Observer (SPO) to output the disturbance estimate Combined with the Jacobian matrix J(x), a dynamic estimation model of the end contact force is constructed, and the estimated end contact force is obtained using the dynamic estimation model of the end contact force;

[0134] Aiming at the heterogeneity of the quick-change therapy probe, the quick-change heterogeneous end effector system updates the mass and inertia parameters in the Jacobian matrix online, and calibrates ΔJ online. p , ensuring that the accuracy of contact force estimation is not affected by changes in mechanical structure. The Jacobian matrix expression is:

[0135]

[0136] Among them, J(x) is the original Jacobian matrix, J p (x) is the generalized Jacobian matrix after parameter correction; ΔJ p is the correction value of dynamic parameters; m p is the quality of the therapy probe; I p is the inertia tensor of the therapy probe; r p is the center of mass position of the therapy probe; ω p is the angular velocity of the distal end of the therapy probe; is the angular acceleration of the end of the therapy probe; g = 9.8 m / s 2 is the acceleration due to gravity;

[0137] The dynamic estimation model of the end contact force maps the joint space disturbance to the Cartesian space through the inverse dynamic solution, thereby indirectly obtaining the contact force information between the physiotherapy probe and the skin. The dynamic estimation model of the end contact force is:

[0138]

[0139] in, is the estimated end contact force; is the disturbance vector;

[0140] Step 4.3.2: Calculate the contact force error based on the desired total contact force of the end of the therapy probe and the estimated end contact force. Use the PID regulator to convert the contact force error into a posture correction value, and implement the outer loop smooth adjustment of the posture correction value.

[0141] The contact force error is:

[0142] In order to achieve smooth regulation of the outer loop of force-position hybrid control, a second-order impedance model is designed first:

[0143]

[0144] Among them, xc is the compliance trajectory; x d is the desired end position; is the expected terminal velocity; is the actual terminal speed; is the expected terminal acceleration; is the actual terminal acceleration; M d 、B d , K d is the inertia, damping and stiffness matrix, which is used to adjust the dynamic response characteristics of the collaborative robot;

[0145] The contact force error is converted into a posture correction through the PID regulator. Its integral term eliminates the steady-state deviation, and the differential term suppresses high-frequency oscillation. The PID regulator formula is:

[0146]

[0147] Where Δx is the steady-state deviation of the collaborative robot’s end position; K fp , K fi , K fd are proportional, integral, and differential adjustment parameters;

[0148] Step 4.3.3: Use inverse kinematics to convert the pose correction into joint commands for the collaborative robot.

[0149] The corrected Cartesian space trajectory x′ is solved by inverse kinematics d Convert to joint instructions:

[0150] θ′ d =IK(x′ d )+Δθ comp (16)

[0151] Where x′ d =x d +Δx is the corrected Cartesian space trajectory; IK(·) is the inverse kinematics solution; is the compliance compensation term, K c is a smooth gain, and θ′ is tracked by the PIDSPO controller d , forming a double closed-loop control.

[0152] Step 5: Design a therapy probe switching strategy;

[0153] The switching of therapy modes is deeply coupled with path planning. The therapy is performed according to the planned preliminary therapy path, and real-time probe switching is performed during the therapy process to achieve rapid switching of therapy modes.

[0154] The specific process of switching the therapy mode is as follows: the collaborative robot first automatically moves the quick-change heterogeneous end effector integrated in the collaborative robot to the quick-change operation area, which is located in an area without any risk of contact with the patient. The physician then manually rotates and unlocks the old therapy probe and replaces it with a new one (the old and new probes are hand-pinch heterogeneous probes 3, finger-pressure heterogeneous probes 5, or palm-press heterogeneous probes 6). No auxiliary tools are required throughout the process. After the replacement is completed, the physician clicks the confirmation command on the external computer to automatically verify the stability of the new therapy probe. The collaborative robot then moves the new therapy probe to the set position in the therapy path, seamlessly connecting the subsequent therapy process. The complete therapy mode switching process takes less than 10 seconds. At the same time, the therapy probe uses a flexible resin material, which effectively avoids mechanical damage and significantly improves the versatility, operating efficiency, and service life of the equipment, fully meeting personalized therapy needs.

[0155] Step 6: Control the collaborative robot and the quick-change heterogeneous end effector integrated in the collaborative robot to perform physical therapy according to the physical therapy goal and the generated physical therapy path. During the physical therapy, the contact force and torque between the quick-change heterogeneous end effector integrated in the collaborative robot and the patient's skin are obtained in real time using the zero-calibrated six-dimensional force sensor 2. The collaborative robot's real-time posture control strategy is used to generate joint instructions for the collaborative robot, dynamically adjust the collaborative robot's posture, and replace the physical therapy probe according to the physical therapy probe switching strategy during the physical therapy.

[0156] When the probe switches from finger pressure mode to palm pressing mode, the quick-change heterogeneous end effector system automatically adjusts the end posture to match the curvature of the human body surface and reduces the contact pressure to expand the range of action; conversely, if it switches to hand pinching mode, the pressure resolution is improved and the contact interval is shortened to enhance local stimulation. During the switching process, the six-dimensional force sensor 2 continuously monitors the change in contact force and fine-tunes the motion trajectory of the probe in combination with the impedance control algorithm to ensure a smooth mode transition without mechanical shock. For example, in shoulder physiotherapy, the quick-change heterogeneous end effector system can first relax the whole body in palm pressing mode, and then automatically switch to hand pinching mode based on palpation feedback to focus on the nodule area.

[0157] During the path planning process, the quick-change heterogeneous end effector system calls the curvature and other data of the skin three-dimensional model in real time, and dynamically optimizes the probe's movement speed, contact depth and action direction (normal / tangential composite motion) in combination with the therapy goals to avoid therapy blind spots or excessive tissue compression caused by path deviation.

[0158] The physical therapy process strictly adheres to the principle of "gradual adaptation." During this process, the quick-change heterogeneous end-effector system dynamically adjusts path parameters to match the patient's tolerance threshold. Initially, a low-frequency (2-3 Hz), low-force (≤5N) palm-pressing mode is used to relax superficial muscles. Subsequently, based on the contact force gradient distribution feedback from the six-dimensional force sensor 2, the application is gradually increased to a medium- to high-frequency (5-10 Hz) finger pressure mode that penetrates the fascia layer. Finally, for stubborn nodules, a high-pressure, focused (15-20N) hand-kneading mode is used.

[0159] Indoor rehabilitation physiotherapy verification:

[0160] The quick-change heterogeneous end effector of the present invention was installed on a collaborative robot for multiple field experiments. The data used in the experiments were compared with the measured values, and it was calculated that the error of the stereoscopic vision depth camera in obtaining the standard normal vector was 3.17%, the maximum error of the quick-change heterogeneous end effector in actively fitting the standard normal vector was 3.23°, and the average error was 2.12°. The average error of the contact force between the end of the quick-change heterogeneous end effector and the skin was 0.47N. It is very necessary to calibrate and filter the six-dimensional force sensor in the experiment. This processing can greatly improve the accuracy and stability of the six-dimensional force sensor data, effectively enhance the constant contact force and shear force between the quick-change heterogeneous end effector and the patient's skin during operation, and significantly improve the sensitivity of the quick-change heterogeneous end effector.

[0161] While ensuring the safety of the experiment, multiple experiments were conducted, and the experiments showed that the quick-change heterogeneous end effector of the present invention, in conjunction with a collaborative robot, can perform a complete rehabilitation therapy integrating multiple therapy modes. The quick-change heterogeneous end effector can quickly develop a targeted and scientifically reasonable therapy path, can quickly identify the autonomous movements of the experimenter, and maintain a constant contact force with the patient's skin. The expected therapy effect was achieved by cross-fusion of data from the six-dimensional force sensor and the stereoscopic vision depth camera, and with the significant advantages of the quick-change mechanism. In the quick-change heterogeneous end effector system, a number of safety settings have been designed and implemented so that it can respond autonomously and effectively to various emergencies during the therapy process, to ensure the safety of patients and operating physicians.

[0162] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A quick-change heterogeneous end effector integrated into a collaborative robot, characterized in that: Includes a stereo vision depth camera, a six-dimensional force sensor, a quick-change mechanism base, and a physical therapy probe; The six-dimensional force sensor is fixed to the top of the physiotherapy probe and is bolted to the end flange of the collaborative robot. It is used to measure the contact force and contact torque between the physiotherapy probe and the patient's skin during the physiotherapy process and send them to an external computer; the stereoscopic vision depth camera is fixed on one side of the six-dimensional force sensor, maintaining a set distance from the axis of the physiotherapy probe, and is used to obtain planar images and point cloud data of the patient's skin surface and send them to an external computer; the quick-change mechanism base is directly fixed to the bottom of the six-dimensional force sensor by bolts, and a buckle groove is provided on it, which cooperates with the rotating buckle on the physiotherapy probe to quickly fix physiotherapy probes of different modes on the quick-change mechanism base. The physiotherapy probe is used to perform different modes of physiotherapy on the patient.

2. The quick-change heterogeneous end effector integrated into a collaborative robot according to claim 1, characterized in that: The external computer flattens the point cloud data acquired by the stereoscopic vision depth camera, and then superimposes it with the plane image to generate a high-precision two-dimensional plane image integrating spatial depth features, and at the same time generates a three-dimensional skin model of the area to be treated based on the acquired point cloud data, and displays the high-precision two-dimensional plane image integrating spatial depth features and the three-dimensional skin model in real time on the screen of the external computer; provides a human-computer interaction function, and the therapist sets the treatment target based on the high-precision two-dimensional plane image integrating spatial depth features and the three-dimensional skin model of the area to be treated displayed on the screen; the external computer plans the path according to the treatment target, and generates control instructions based on the treatment path, the joint angles of the collaborative robot in real time, the coordinates and normal direction of the center point of the end flange, and the measured contact force and contact torque between the treatment probe and the patient's skin, and sends them to the collaborative robot and the treatment probe; the treatment target includes the treatment range and the treatment plan, and the treatment plan includes the sequence of various treatment modes during the treatment process, as well as the working frequency and contact force size under each treatment mode.

3. The quick-change heterogeneous end effector integrated into a collaborative robot according to claim 1, characterized in that: The physiotherapy probes include a hand-kneading isomeric probe, a finger-pressure isomeric probe, and a palm-pressing isomeric probe, which correspond to the hand-kneading mode, finger-pressure mode, and palm-pressing mode during physiotherapy, respectively; A rotating buckle is set on the top of the hand-pinch heterogeneous probe, and two groups of symmetrically distributed bionic four-finger grippers are set at the bottom. The hand-pinch heterogeneous probe has a built-in screw stepper motor and a single-chip microcomputer. The screw stepper motor drives the two groups of symmetrically distributed bionic four-finger grippers to open and close to complete the hand-pinch action; the single-chip microcomputer is used to receive control instructions from an external computer and dynamically adjust the opening and closing, massage strength and working frequency of the hand-pinch heterogeneous probe; the finger pressure heterogeneous probe and the palm press heterogeneous probe have rotating buckles of the same size on the top, and the bottom of the finger pressure heterogeneous probe is a thumb-shaped structure; the palm press heterogeneous probe is a semi-ellipsoidal curved surface structure.

4. The quick-change heterogeneous end effector integrated into a collaborative robot according to claim 1, characterized in that: The six-dimensional force sensor and the external computer use USB serial communication to send the measured contact force and torque between the physiotherapy probe and the patient's skin to the external computer in real time; The built-in single-chip microcomputer of the hand-pinch heterogeneous probe is connected to the port of an external computer via USB Type-C to receive control instructions.

5. The quick-change heterogeneous end effector integrated into a collaborative robot according to claim 1, characterized in that: When the external computer detects that the contact force and torque between the therapy probe and the patient's skin are greater than the set threshold, the therapy process is stopped immediately; a manual operation function is provided on the screen of the external computer, including manual adjustment of the therapy mode and the size of the contact force, and the manual operation has a higher priority than the control instructions generated by the external computer. At the same time, an emergency stop button is set on the screen to stop the therapy process at any time.

6. A control method for a quick-change heterogeneous end effector integrated in a collaborative robot, based on the quick-change heterogeneous end effector integrated in the collaborative robot, characterized in that: The following steps are involved: Using stereo vision depth cameras to identify therapy targets; Calibrate and zero the six-dimensional force sensor; Generate a therapy pathway based on the set therapy goals; Design a real-time control strategy for the collaborative robot's posture, which is used to generate joint instructions for the collaborative robot based on the therapy path, the joint angles of the collaborative robot, the coordinates and normal direction of the end flange center point, and the measured contact force and contact torque between the therapy probe and the patient's skin; Design therapy probe switching strategies; According to the therapy goals and the generated therapy path, the collaborative robot and the quick-change heterogeneous end effector integrated in the collaborative robot are controlled to perform therapy. During the therapy, the contact force and torque between the quick-change heterogeneous end effector integrated in the collaborative robot and the patient's skin are obtained in real time using the zero-calibrated six-dimensional force sensor. The collaborative robot's real-time posture control strategy is used to generate joint instructions for the collaborative robot, dynamically adjust the posture of the collaborative robot, and replace the therapy probe according to the therapy probe switching strategy during the therapy.

7. The control method of the quick-change heterogeneous end effector integrated into the collaborative robot according to claim 6, characterized in that: The physical therapy path is generated according to the set physical therapy goal, specifically: For the acupressure mode, the generated therapy path is a number of discrete acupuncture points, and also includes the movement speed of the acupressure heterogeneous probe, the dwell time at each discrete acupuncture point, and the pressing depth; For the palm-press mode, the generated therapy path is a continuous trajectory, which also includes the tilt angle, movement speed and expected contact force of the palm-press heterogeneous probe; For the hand-kneading mode, the generated therapy path is a continuous trajectory, which cycles in a contact-hover-recontact pattern with the skin at a set speed, with intermittent kneading actions.

8. The control method of the quick-change heterogeneous end effector integrated into the collaborative robot according to claim 6, characterized in that: The real-time control strategy for the collaborative robot's posture is specifically designed to include: S1: Construct joint dynamics model; Wherein, j represents the joint number of the collaborative robot, i represents the joint number of the collaborative robot different from j, k represents the number of joints of the collaborative robot, and n represents the number of joints other than the j-th joint; is the state vector of the collaborative robot; x j represents the angle of the j-th joint of the collaborative robot; represents the angular velocity of the jth joint of the collaborative robot; represents the angular acceleration of the jth joint of the collaborative robot; f j (x) is the known nonlinear dynamics; Δf j (x) is the model uncertainty; b ji (x) is the control gain matrix; Δb ji (x) is the gain perturbation; u i is the control input; d j (t) is the external disturbance; t represents the time that changes with the treatment process; S2: Design an SPO active disturbance rejection observer based on the joint dynamics model; the SPO active disturbance rejection observer is used to reconstruct the total disturbance in real time, obtain the disturbance estimate, and then realize disturbance compensation; The dynamic equation of the active disturbance rejection observer is: in, is the estimated position of the end of the therapy probe; is the estimated velocity of the therapy probe tip; is the estimated value of the disturbance at the end of the therapy probe; is the estimated rate of change of the position of the distal end of the therapy probe; is the estimated acceleration of the physiotherapy probe tip; is the disturbance change rate of the end of the therapy probe; α 1j , α 2j is the linearization gain; k 1j and k 2j is the error feedback gain; is the position estimation error; is the speed estimation error; is the estimated value of the total disturbance of the quick-change heterogeneous end effector; sat(·) is the boundary layer saturation function, which is defined as: Among them, ε 0j is the boundary layer thickness; the sign(·) function is a piecewise function; The final disturbance estimate of the ADRO output is The error feedback gain of the ADRO must satisfy the stability condition: Among them, λ dj is the dominant pole parameter; S3: Combine the ADRO designed in S2 with a PID controller to construct a dual-closed-loop PIDSPO controller, which is then used to generate joint commands for the collaborative robot. The dual-closed-loop PIDSPO controller includes a feedforward channel and a feedback channel. The feedforward channel actively cancels disturbances using the disturbance estimate output by the ADRO, significantly reducing the regulation burden of the feedback channel. The feedback channel uses a classic PID controller to eliminate residual disturbances and modeling errors. The double closed-loop control law formula of the double closed-loop PIDSPO controller is: Among them, F d is the desired total contact force at the end of the therapy probe; J j is the Jacobian matrix corresponding to the three-dimensional coordinates of the collaborative robot and the joint space; τ is the time variable for integration and differentiation operations; k pj 、k ij 、k dj is the PID gain; m j is the equivalent inertia parameter; B j is the linear damping coefficient; e j is the tracking error.

9. The control method of the quick-change heterogeneous end effector integrated into the collaborative robot according to claim 8, characterized in that: The joint instructions of the collaborative robot generated by the double closed-loop PIDSPO controller are specifically: S3.1: Output disturbance estimates using an active disturbance rejection observer Combined with the Jacobian matrix J(x), a dynamic estimation model of the end contact force is constructed, and the estimated end contact force is obtained using the dynamic estimation model of the end contact force; First, the kinetic parameter correction value ΔJ is calibrated online p , modify the Jacobian matrix J(x): Among them, J(x) is the original Jacobian matrix, J p (x) is the generalized Jacobian matrix after parameter correction; ΔJ p is the correction value of dynamic parameters; m p is the quality of the therapy probe; I p is the inertia tensor of the therapy probe; r p is the center of mass position of the therapy probe; ω p is the angular velocity of the distal end of the therapy probe; is the angular acceleration of the end of the therapy probe; g = 9.8 m / s 2 is the acceleration due to gravity; The dynamic estimation model of the end contact force is: in, is the estimated end contact force; is the disturbance vector; S3.2: Calculate the contact force error based on the expected total contact force of the end of the therapy probe and the estimated end contact force, convert the contact force error into a posture correction value using a PID regulator, and implement outer loop smooth adjustment of the posture correction value; The contact force error is: The PID regulator formula is: Where Δx is the steady-state deviation of the collaborative robot’s end position; K fp , K fi , K fd are proportional, integral, and differential adjustment parameters; S3.3: Use inverse kinematics to convert pose corrections into joint commands for the collaborative robot. The corrected Cartesian space trajectory x′ is solved by inverse kinematics d Convert to joint instructions: θ′ d =IK(x′ d )+Δθ comp (16) Where x′ d =x d +Δx is the corrected Cartesian space trajectory, x d is the desired end pose; IK(·) is the inverse kinematics solution; is the compliance compensation term, K c is a smooth gain, and θ′ is tracked by the PIDSPO controller d , forming a double closed-loop control.

10. The control method of the quick-change heterogeneous end effector integrated into the collaborative robot according to claim 6, characterized in that: The specific strategy for switching the therapy probe is as follows: The collaborative robot first automatically moves the quick-change heterogeneous end effector integrated in the collaborative robot to the quick-change operation area, which is located in an area without any risk of contact with the patient. It then rotates to unlock the old therapy probe and replaces it with a new one. After the replacement is completed, click the confirmation command on the external computer, and then use the collaborative robot to move the new therapy probe to the set position in the therapy path, seamlessly connecting the subsequent therapy process.

Images