A control method and system for a dual-machine collaborative upper limb rehabilitation robot

Abstract

This invention belongs to the field of robot-assisted rehabilitation technology and discloses a control method and system for a dual-robot collaborative upper limb rehabilitation robot. The invention obtains an auxiliary force through externally applied interactive force and the guiding force required to move to the target position. The auxiliary force and interactive force are then substituted into an admittance control model to obtain the desired speed of the robot, thereby completing the collaborative or resistance task between the two robots. This invention can provide stroke patients with comprehensive upper limb rehabilitation exercises, which is conducive to stimulating patients' active participation, improving the efficiency of rehabilitation training, and promoting brain function and neural remodeling.

Description

Technical Field

[0001] This invention belongs to the field of robot-assisted rehabilitation technology, specifically relating to a control method and system for a dual-machine collaborative upper limb rehabilitation robot. Background Technology

[0002] Stroke is a common neurological disorder, also known as cerebrovascular accident. According to the World Health Organization, China has one of the highest stroke rates in the world, with the vast majority of survivors experiencing varying degrees of disability, most commonly hemiplegia. Among stroke patients, over 80% suffer from some degree of upper limb motor function limitation, severely impacting their quality of life. With the global aging population and the increasing number of patients with limb motor dysfunction due to nerve damage, meeting the enormous rehabilitation needs and improving patients' quality of life has become a critical issue that urgently requires resolution.

[0003] Integrating robotics and related technologies with clinical rehabilitation medicine to design rehabilitation robots that can replace physicians in patient rehabilitation training can effectively alleviate physician stress. Simultaneously, patients can utilize robotic technology to acquire highly repetitive movements, aiding and enhancing their rehabilitation. Currently, existing upper limb rehabilitation robot systems only involve interaction between a single robot and the patient. Although researchers have designed engaging games to increase patient participation during rehabilitation training, patients often find it tedious and stop training after a period, failing to maintain training intensity and significantly reducing the effectiveness of rehabilitation training. This, in turn, affects the patient's rehabilitation progress and may cause them to miss the optimal rehabilitation period. Compared to existing rehabilitation robots, dual-robot collaborative rehabilitation robots can achieve various forms of rehabilitation training, such as patient-patient and doctor-patient interactions, enabling patients to achieve greater participation and more exercise.

[0004] Therefore, based on the above problems, it is necessary to improve the traditional upper limb rehabilitation robot system that only involves interaction between a single robot and the patient, so as to increase the patient's active participation and training efficiency. Summary of the Invention

[0005] In view of the problems and shortcomings of the existing technology, the purpose of this invention is to provide a control method and system for a dual-machine collaborative upper limb rehabilitation robot.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] The first aspect of this invention provides a control method for a dual-machine collaborative upper limb rehabilitation robot, comprising the following steps:

[0008] (1) Obtain the real-time position and interaction force of the two robotic arms in their own coordinate system;

[0009] (2) Convert the current real-time position of the end of the robotic arm in its own coordinate system into coordinate values ​​relative to the coordinate system of the other robotic arm, and record the coordinate values ​​as the target position of the end of the other robotic arm. Then generate a guiding force acting on the end of the other robotic arm based on the real-time position of the end of the other robotic arm and the target position of the end of the other robotic arm; and obtain the guiding force of the current end of the robotic arm according to the above method.

[0010] (3) Obtain the final resultant force of the current robotic arm end based on the guiding force of the current robotic arm end and the interaction force of the other robotic arm end; and obtain the final resultant force of the other robotic arm end according to the above method; then constrain the final resultant force of the two robotic arm ends respectively according to the motion range boundary model to obtain the auxiliary force of the two robotic arm ends respectively.

[0011] (4) Generate the desired speed of each of the two robotic arms based on their own interaction force and their own auxiliary force at the ends of the two robotic arms, so that the two robotic arms can move in cooperation.

[0012] The interaction force is applied from the outside. When the interaction forces applied by the users at the ends of the two robotic arms are in the same direction, the users at the ends of the two robotic arms are controlled to form a coordinated movement. When the interaction forces applied by the users at the ends of the two robotic arms are in different directions, the interaction force of the current robotic arm is controlled to prevent the other robotic arm from moving to the target position, and the users at the ends of the two robotic arms are controlled to form a resistive movement.

[0013] Step (2) can be further explained as follows: The real-time position of the first robotic arm's end effector in its own coordinate system is converted into a first coordinate value relative to the second robotic arm's own coordinate system, and this first coordinate value is recorded as the target position of the second robotic arm's end effector. Then, a guiding force acting on the second robotic arm's end effector is generated based on the real-time position and the target position of the second robotic arm's end effector. Simultaneously, the real-time position of the second robotic arm's end effector in its own coordinate system is converted into a second coordinate value relative to the first robotic arm's own coordinate system, and this second coordinate value is recorded as the target position of the first robotic arm's end effector. Then, a guiding force acting on the first robotic arm's end effector is generated based on the real-time position and the target position of the first robotic arm's end effector. This yields the guiding forces of each of the two robotic arm ends.

[0014] Preferably, before recording the coordinate values ​​as the target position of the robotic arm's end effector, the coordinate values ​​need to be constrained according to the abstract motion boundary to obtain constrained coordinate values. The specific steps of the constraint process are as follows: the position coordinates in the x-axis direction are restricted to the range of [-m, +m], and the position coordinates in the y-axis and z-axis directions are restricted to the range of [0, m], where m is a constant. Furthermore, the constrained coordinate values ​​are subjected to coordinate scaling transformation to ensure the relative accuracy of the positions of the two robotic arms when they move within different motion ranges.

[0015] Furthermore, the guiding force described in step (2) includes both magnitude and direction.

[0016] Preferably, the step of generating the direction of the guiding force specifically involves: converting the real-time position of the current robotic arm end in its own coordinate system into coordinate values ​​relative to the coordinate system of another robotic arm, and recording the coordinate values ​​as the target position of the other robotic arm end; then, determining the direction of the guiding force of the other robotic arm end as the direction of the real-time position of the other robotic arm end pointing to the target position of the other robotic arm end in the same coordinate system; and obtaining the direction of the guiding force of the current robotic arm end according to the above method.

[0017] Preferably, the step S10 of generating the magnitude of the guiding force specifically comprises:

[0018] S11, In the same coordinate system, obtain the real-time position of the robotic arm's end effector in its own coordinate system and the target position, and calculate the distance between them according to the following formula.

[0019]

[0020] In the above formula, For the current position of robotic arm A from its own real-time position P A To the target location P B The distance between them, where P A =[x A y A z A ] T x A y A z A The current real-time position P of robotic arm A is... A The three-dimensional coordinates of P B =[x B y B z B ] T x B y B z B The target position P of the current robotic arm A.B The three-dimensional coordinates;

[0021] S12, Substitute the distance obtained from S11 into the following formula to obtain the magnitude of the guiding force at the end of the robotic arm.

[0022]

[0023] In the above formula, F gA This represents the magnitude of the guiding force of the current robotic arm A. For the current position of robotic arm A from its own real-time position P A Pointing to the target location P B The vector; r is the threshold of the unguided force range; S A Let be the virtual stiffness of the current robotic arm A.

[0024] More preferably, the virtual stiffness S is 0-1000, excluding 1000; more preferably 480-1000, excluding 1000. It should be noted that the virtual stiffness S... A The subscript A in the text represents the current robotic arm or any one of the two robotic arms, and the virtual stiffness is represented by S.

[0025] Preferably, in step (3), step S20, which obtains the final resultant force of the current robotic arm end based on the guiding force of the current robotic arm end and the interaction force of the other robotic arm end, specifically involves:

[0026] S21, Determine the initial resultant force: Substitute the guiding force at the current end of the robotic arm and the interaction force at the end of the other robotic arm into the following formula and perform vector summation to obtain the initial resultant force at the current end of the robotic arm.

[0027] F r1A =F gA +F iB

[0028] In the above formula, F r1A F is the initial net force at the end of the current robotic arm A, and is a vector; gA F is the guiding force at the end of the current robotic arm A. iB The interaction force received by the current robotic arm A from the end of another robotic arm B;

[0029] S22, Determine the final resultant force: Substitute the initial resultant force of the current robotic arm end effector obtained in S21 into the following formula to obtain the final resultant force of the current robotic arm end effector.

[0030] F r2 =min(F r1 ,F max )

[0031] In the above formula, F r2 F is the final resultant force at the end effector of the current robotic arm.r1 F is the initial net force at the end effector of the robotic arm. max The maximum force applied to the current end effector of the robotic arm can be set by the doctor based on the patient's recovery progress.

[0032] Step (3) can be further explained as follows: the final resultant force of the first robotic arm end is obtained based on the guiding force of the first robotic arm end and the interaction force of the second robotic arm end; the final resultant force of the second robotic arm end is obtained based on the guiding force of the second robotic arm end and the interaction force of the first robotic arm end; and then the final resultant force of each of the two robotic arm ends is constrained according to the motion range boundary model to obtain the auxiliary force of each of the two robotic arm ends.

[0033] Preferably, in step (3), step S30, which involves constraining the final resultant force at the end of the robotic arm based on the motion range boundary model, specifically involves:

[0034] S31, first calculate the axial direction components of the interaction force direction and motion trend direction of the end of the robotic arm on the xyz three axes, and then compare the similarities and differences of the axial direction components on different coordinate axes.

[0035] When the axial component of the interaction force direction is in the same direction as the axial component of the motion trend direction, the axial component of the final resultant force at the end of the robotic arm along that coordinate axis is substituted into the motion range boundary model formula to obtain the axial component of the auxiliary force at the end of the robotic arm along that coordinate axis. The motion range boundary model formula is shown below.

[0036]

[0037] In the above formula, P is the real-time position of the current robotic arm end effector; D1 is the normal range of motion of the current robotic arm end effector; D2 is the rigid range of motion of the current robotic arm end effector, which is the protection range surrounding the normal range of motion. Within this protection range, the auxiliary force gradually reduces the final resultant force to 0 according to the change in the distance from the real-time position to the boundary; C is a constant, the specific value of which is determined by the interaction force of the current robotic arm itself. D1 and D2 are both obtained relative to the robotic arm's own coordinate system.

[0038] When the axial component of the interaction force direction is opposite to the axial component of the motion trend direction, the axial component of the final resultant force at the end of the robotic arm along the coordinate axis is denoted as the axial component of the auxiliary force at the end of the robotic arm along the coordinate axis.

[0039] S32, sum the axial component vectors of the auxiliary force at the end of the robotic arm along the x, y, and z axes to obtain the auxiliary force at the end of the robotic arm.

[0040] Preferably, in step (4), the interaction force and auxiliary force at the end of the robotic arm generate the desired speed of the robotic arm through the admittance controller. Further, step S40, which generates the desired speed of each end of the two robotic arms based on their own interaction force and auxiliary force, specifically involves substituting the interaction force and auxiliary force of each end of the robotic arm into the admittance control model formula of the admittance controller to obtain the desired speed of the robotic arm. The admittance control model formula is specifically shown below.

[0041]

[0042] In the above formula, F aA F is the auxiliary force at the end of the current robotic arm A. iA M represents the interaction force at the end effector of robotic arm A. A D is the inertia matrix; A K is the damping matrix; A Here is the stiffness matrix; Δx = x0 - x d , where x d , Let x0 be the robot's desired pose, velocity, and acceleration. This refers to the position, velocity, and acceleration values ​​that the robot theoretically needs to track when the external force is zero.

[0043] A second aspect of the present invention provides a dual-machine collaborative upper limb rehabilitation robot system, comprising:

[0044] The position and interaction force acquisition module is used to acquire the real-time position and interaction force of each of the two robotic arms in its own coordinate system.

[0045] A guiding force determination module is provided, the input of which is connected to the output of the position and interaction force acquisition module. The guiding force determination module acquires the real-time positions of the two robotic arm ends in their respective coordinate systems, as output by the position and interaction force acquisition module. Then, it converts the real-time position of the first robotic arm end in its own coordinate system into a first coordinate value relative to the second robotic arm's own coordinate system, and records the first coordinate value as the target position of the second robotic arm end. Based on the real-time position and the target position of the second robotic arm end, a guiding force is generated acting on the second robotic arm end. Simultaneously, the guiding force determination module also converts the real-time position of the second robotic arm end in its own coordinate system into a second coordinate value relative to the first robotic arm's own coordinate system, records the second coordinate value as the target position of the first robotic arm end, and generates a guiding force acting on the first robotic arm end based on the real-time position and the target position of the first robotic arm end.

[0046] The constrained system's auxiliary force determination module has its input terminals connected to the output terminals of the position and interaction force acquisition module and the guiding force determination module, respectively. This module acquires the interaction forces of the two robotic arm ends output by the position and interaction force acquisition module and the guiding forces of the two robotic arm ends output by the guiding force determination module. Then, based on the guiding force of the first robotic arm end and the interaction force of the second robotic arm end, it obtains the final resultant force of the first robotic arm end, and simultaneously, based on the guiding force of the second robotic arm end and the interaction force of the first robotic arm end, it obtains the final resultant force of the second robotic arm end. Finally, based on the motion range boundary model, it constrains the final resultant forces of the two robotic arm ends to obtain the auxiliary forces of each robotic arm end.

[0047] The robotic arm execution module has its input terminals connected to the output terminals of the position and interaction force acquisition module and the auxiliary force determination module of the constrained system, respectively. The robotic arm execution module acquires the interaction forces of the two robotic arm ends output by the position and interaction force acquisition module and the auxiliary forces of the two robotic arm ends output by the auxiliary force determination module of the constrained system. Then, based on the interaction forces and auxiliary forces of the two robotic arm ends, it generates the desired velocities of the two robotic arms, enabling them to move collaboratively.

[0048] Furthermore, the communication channels between the position and interaction force acquisition module and the guiding force determination module, between the auxiliary force determination module of the constrained system and the position and interaction force acquisition module and the guiding force determination module, and between the robotic arm execution module and the position and interaction force acquisition module and the auxiliary force determination module of the constrained system, also include communication channels for position information and / or the magnitude and direction of the force.

[0049] Preferably, the position and interaction force acquisition module converts the joint angles of the robotic arm into position information through forward kinematics, thereby obtaining the real-time position of the robotic arm end effector; the position and interaction force acquisition module acquires the interaction force of the robotic arm end effector through a six-dimensional force sensor.

[0050] Preferably, the guiding force determination module includes a guiding force direction determination unit and a guiding force magnitude determination unit, wherein the guiding force direction determination unit is used to generate the guiding force direction at the end of the robotic arm, and the guiding force magnitude determination unit is used to generate the guiding force magnitude at the end of the robotic arm.

[0051] Preferably, the specific steps of the guiding force direction determination unit to generate the guiding force direction of the robotic arm end are as follows: converting the current real-time position of the robotic arm end in its own coordinate system into coordinate values ​​relative to the coordinate system of another robotic arm, and recording the coordinate values ​​as the target position of the other robotic arm end, and then determining the guiding force direction of the other robotic arm end in the same coordinate system based on the real-time position of the other robotic arm end and the target position of the other robotic arm end.

[0052] Preferably, before recording the coordinate values ​​as the target position of the robotic arm's end effector, the coordinate values ​​need to be constrained according to the abstract motion boundary to obtain constrained coordinate values. The specific steps of the constraint process are as follows: the position coordinates in the x-axis direction are restricted to the range of [-m, +m], and the position coordinates in the y-axis and z-axis directions are restricted to the range of [0, m], where m is a constant. Furthermore, the constrained coordinate values ​​are subjected to coordinate scaling transformation to ensure the relative accuracy of the positions of the two robotic arms when they move within different motion ranges.

[0053] Further, the specific steps of the guiding force direction determination unit in generating the guiding force direction of the robotic arm end effector are as follows: The acquired real-time position of the first robotic arm end effector in its own coordinate system is converted into a first coordinate value relative to the second robotic arm's own coordinate system, and this first coordinate value is recorded as the target position of the second robotic arm end effector. Then, in the same coordinate system, the direction of the guiding force of the second robotic arm end effector is determined by pointing from its real-time position to the target position of the second robotic arm end effector. Simultaneously, the guiding force direction determination unit is also used to convert the real-time position of the second robotic arm end effector in its own coordinate system into a second coordinate value relative to the first robotic arm's own coordinate system, and this second coordinate value is recorded as the target position of the first robotic arm end effector. Then, in the same coordinate system, the direction of the guiding force of the first robotic arm end effector is determined by pointing from its real-time position to the target position of the first robotic arm end effector.

[0054] Preferably, step S10 of the guiding force magnitude determination unit generating the guiding force magnitude at the end of the robotic arm specifically includes:

[0055] S11, obtain the real-time position of the robotic arm's end effector in its own coordinate system and the target position from the direction determination unit of the guiding force, and obtain the distance between the two according to the following formula.

[0056]

[0057] In the above formula, For the current position of robotic arm A from its own real-time position P A To the target location PB The distance between them, where P A =[x A y A z A ] T x A y A z A The current real-time position P of robotic arm A is... A The three-dimensional coordinates of P B =[x B y B z B ] T x B y B z B The target position P of the current robotic arm A. B The three-dimensional coordinates;

[0058] S12, Substitute the distance obtained from S11 into the following formula to obtain the magnitude of the guiding force at the end of the robotic arm.

[0059]

[0060] In the above formula, F gA This represents the magnitude of the guiding force of the current robotic arm A. For the current position of robotic arm A from its own real-time position P A Pointing to the target location P B The vector; r is the threshold of the unguided force range; S A Let be the virtual stiffness of the current robotic arm A.

[0061] More preferably, the virtual stiffness S is 0-1000, excluding 1000; more preferably 480-1000, excluding 1000. It should be noted that S... A The subscript A in the text represents the current robotic arm or any one of the two robotic arms, and the virtual stiffness is represented by S.

[0062] Preferably, step S20, which involves obtaining the final resultant force of the current robotic arm end effector based on the guiding force of the current robotic arm end effector and the interaction force of the other robotic arm end effector, specifically comprises:

[0063] S21, Determine the initial resultant force: Substitute the guiding force at the current end of the robotic arm and the interaction force at the end of the other robotic arm into the following formula and perform vector summation to obtain the initial resultant force at the current end of the robotic arm.

[0064] F r1A =F gA +F iB

[0065] In the above formula, Fr1A F is the initial net force at the end of the current robotic arm A, and is a vector; gA F is the guiding force at the end of the current robotic arm A. iB The interaction force received by the current robotic arm A from the end of another robotic arm B;

[0066] S22, Determine the final resultant force: Substitute the initial resultant force of the current robotic arm end effector obtained in S21 into the following formula to obtain the final resultant force of the current robotic arm end effector.

[0067] F r2 =min(F r1 ,F max )

[0068] In the above formula, F r2 F is the final resultant force at the end effector of the current robotic arm. r1 F is the initial net force at the end effector of the robotic arm. max The maximum force applied to the current end effector of the robotic arm can be set by the doctor based on the patient's recovery progress.

[0069] Preferably, step S30, which involves constraining the final resultant force at the end of the robotic arm based on the motion range boundary model to obtain the auxiliary force at the end of the robotic arm, specifically comprises:

[0070] S31, first calculate the axial direction components of the interaction force direction and motion trend direction of the end of the robotic arm on the xyz three axes, and then compare the similarities and differences of the axial direction components on different coordinate axes.

[0071] When the axial component of the interaction force direction is in the same direction as the axial component of the motion trend direction, the axial component of the final resultant force at the end of the robotic arm along that coordinate axis is substituted into the motion range boundary model formula to obtain the axial component of the auxiliary force at the end of the robotic arm along that coordinate axis. The motion range boundary model formula is shown below.

[0072]

[0073] In the above formula, P is the real-time position of the current robotic arm end effector; D1 is the normal range of motion of the current robotic arm end effector; D2 is the rigid range of motion of the current robotic arm end effector, which is the protection range surrounding the normal range of motion. Within this protection range, the auxiliary force gradually reduces the final resultant force to 0 according to the change in the distance from the real-time position to the boundary; C is a constant, the specific value of which is determined by the interaction force of the current robotic arm itself. D1 and D2 are both obtained relative to the robotic arm's own coordinate system.

[0074] When the axial component of the interaction force direction is opposite to the axial component of the motion trend direction, the axial component of the final resultant force at the end of the robotic arm along the coordinate axis is denoted as the axial component of the auxiliary force at the end of the robotic arm along the coordinate axis.

[0075] S32, sum the axial component vectors of the auxiliary force at the end of the robotic arm along the x, y, and z axes to obtain the auxiliary force at the end of the robotic arm.

[0076] Preferably, in the robotic arm execution module, the interaction force and auxiliary force at the end of the robotic arm generate the desired velocity of the end of the robotic arm through an admittance controller; step S40, which generates the desired velocity of the end of the robotic arm through the interaction force and auxiliary force, specifically involves substituting the interaction force and auxiliary force of the end of the robotic arm itself into the admittance control model formula of the admittance controller to obtain the desired velocity of the end of the robotic arm, wherein the admittance control model formula is specifically as follows.

[0077]

[0078] In the above formula, F aA F is the auxiliary force at the end of the current robotic arm A. iA M represents the interaction force at the end effector of robotic arm A. A D is the inertia matrix; A K is the damping matrix; A Here is the stiffness matrix; Δx = x0 - x d , where x d , Let x0 be the robot's desired pose, velocity, and acceleration. This refers to the position, velocity, and acceleration values ​​that the robot theoretically needs to track when the external force is zero.

[0079] A third aspect of the present invention provides a computer-readable storage medium storing a computer program, the computer program including the dual-machine collaborative upper limb rehabilitation robot system described in the second aspect above; and / or the computer program, when executed by a processor, implements any step of the control method for the dual-machine collaborative upper limb rehabilitation robot described in the first aspect above.

[0080] A fourth aspect of the present invention provides an electronic device, including a memory and a processor, wherein the memory stores a computer program; the processor executes the computer program; the computer program includes the dual-machine collaborative upper limb rehabilitation robot system described in the second aspect above; and / or the processor, when executing the computer program, implements any step of the control method of the dual-machine collaborative upper limb rehabilitation robot as described in the first aspect above.

[0081] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0082] (1) This invention obtains an auxiliary force by applying an external interaction force and a guiding force required to move to the target position. The auxiliary force and guiding force are then substituted into the admittance control model to obtain the robot's desired speed, thereby completing the cooperative or resistance-resistant task between the two robots. Furthermore, this invention calculates the desired speed using the auxiliary force obtained by summing the interaction force and the guiding force, which allows the motion trajectories of the two robots to be more closely aligned and the deviation to be smaller. In one embodiment, compared to directly using the guiding force as the auxiliary force, the robot's motion trajectory deviation is reduced by approximately 0.01m.

[0083] (2) This invention calculates the guiding force using the real-time position of another robot as the target position for the current robot. During the calculation, it was found that the stiffness coefficient in the guiding force formula has a significant impact on the deviation of the robot's motion trajectory. The smaller the stiffness coefficient, the larger the trajectory deviation; as the stiffness coefficient increases, the trajectory deviation decreases. However, the stiffness coefficient cannot be increased indefinitely, because an excessively large stiffness coefficient will cause the robotic arm to vibrate, resulting in a large trajectory deviation and affecting motion synchronization.

[0084] (3) The control method of the dual-machine collaborative upper limb rehabilitation robot of the present invention enables two patients or one patient and one doctor to complete rehabilitation training tasks together with less error through collaboration or resistance, which is more interesting and interactive, enhances the attractiveness of the treatment process, improves the patient's active participation, better promotes the remodeling of the damaged motor function area of ​​the brain, improves the efficiency of upper limb motor function rehabilitation training and treatment efficiency for stroke patients, and enables patients to regain some daily living self-care ability.

[0085] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0086] Figure 1 This is a flowchart illustrating the control method for the dual-machine collaborative upper limb rehabilitation robot of the present invention;

[0087] Figure 2 This is a basic control principle diagram of the dual-machine collaborative upper limb rehabilitation robot control method of the present invention;

[0088] Figure 3 This is a schematic diagram of the mass-spring-damping model of the admittance controller of the present invention;

[0089] Figure 4Three-dimensional diagrams of the motion trajectories of the robotic arm end effector of the present invention under the control methods of Embodiment 1 and Comparative Example 1, respectively;

[0090] Figure 5 The diagram shows the motion trajectory deviation of the end effector of the robotic arm of the present invention under the control methods of Embodiment 1 and Comparative Example 1, respectively; in the diagram, the red solid line and the purple dashed line represent Embodiment 1, and the blue solid line and the yellow dashed line represent Comparative Example 1;

[0091] Figure 6 The diagram shows the motion trajectory deviation of the end effector of the robotic arm of the present invention under the control methods of Example 1 and Comparative Examples 2-3, respectively. Detailed Implementation

[0092] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0093] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0094] Example 1

[0095] This embodiment provides a control method for a dual-machine collaborative upper limb rehabilitation robot, such as... Figure 1 and Figure 2 As shown, it includes the following steps:

[0096] (1) The current real-time position and end-effector interaction force of robotic arms A and B are obtained respectively and transmitted to the other robotic arm through the communication channel; the three-dimensional coordinate value of the real-time position is obtained by calculating the joint angle of the robotic arm through forward kinematics; the interaction force is measured by the six-dimensional force sensor at the end of the robotic arm and obtained by the written algorithm.

[0097] It should be noted that the real-time position and interaction force of the robotic arm's end effector are calculated relative to its own coordinate system; the robotic arm's own coordinate system is the base coordinate system of the robotic arm, which is established with the center point of the robotic arm's base as the origin.

[0098] (2) The real-time position of the end of robotic arm A in its own coordinate system is converted into a coordinate value A relative to the coordinate system of robotic arm B, and the coordinate value A is recorded as the target position of the end of robotic arm B. Then, a guiding force acting on the end of robotic arm B is generated based on the real-time position of the end of robotic arm B and the target position of the end of robotic arm B. At the same time, the real-time position of the end of robotic arm B in its own coordinate system is converted into a coordinate value B relative to the coordinate system of robotic arm A, and the coordinate value B is recorded as the target position of the end of robotic arm A. Then, a guiding force acting on the end of robotic arm A is generated based on the real-time position of the end of robotic arm A and the target position of the end of robotic arm A.

[0099] Before assigning coordinate values ​​A or B as the target position of the robotic arm's end effector, constraints need to be applied to A or B based on abstract motion boundaries to obtain constrained coordinate values ​​A or B. The specific steps of the constraint process are as follows: the x-axis coordinates of A or B are restricted to the range [-m, +m], and the y-axis and z-axis coordinates are restricted to the range [0, m], where m is a constant. Furthermore, coordinate scaling transformations are performed on the constrained coordinate values ​​A or B to ensure the relative accuracy of the positions of the two robotic arms when they move within different motion ranges.

[0100] Since guiding force includes both magnitude and direction, they need to be calculated separately.

[0101] Specifically, the step of generating the direction of the guiding force is as follows: the direction of the guiding force is the direction from the real-time position coordinates of the robotic arm in its own coordinate system to the coordinates of the target position.

[0102] The specific steps for generating the magnitude of the guiding force are as follows:

[0103] For robotic arm A, the distance between the two is first calculated based on the real-time position coordinates obtained relative to robotic arm A's own coordinate system and the target position coordinates. Then distance Substituting into the following formula, we obtain the magnitude of the guiding force at the end of robotic arm A.

[0104]

[0105] In the above formula, F gA This represents the magnitude of the guiding force on side A of the current robotic arm; For the current position of robotic arm A from its own real-time position P A Pointing to the target location P B ; For the current position of robotic arm A from its own real-time position P A To the target location P B The distance between them, where P A =[xA y A z A ] T x A y A z A The current real-time position P of robotic arm A is... A The three-dimensional coordinates of P B =[x B y B z B ] T x B y B z B The target position P of the current robotic arm A. B The three-dimensional coordinates of the value; r is the threshold of the range without guiding force; S A The virtual stiffness of the current robotic arm A is set to 480 in this embodiment.

[0106] For robotic arm B, the distance between the two is first calculated based on the real-time position coordinates obtained relative to robotic arm B's own coordinate system and the target position coordinates. Then distance Substituting into the following formula, we obtain the magnitude of the guiding force at the end of robotic arm B.

[0107]

[0108] In the above formula, F gB This represents the magnitude of the guiding force on side B of the current robotic arm; For the current position of robotic arm B from its own real-time position P B Pointing to the target location P A ; For the current position of robotic arm B from its own real-time position P A To the target location P B The distance between them, where P B =[x B y B z B ] T x B y B z B The current real-time position P of robotic arm B is... B The three-dimensional coordinates of P A =[x A y A z A ] T x A y A z AThe target position P of the current robotic arm B. A The three-dimensional coordinates of the value; r is the threshold of the range without guiding force; S B The virtual stiffness of the current robotic arm B is set to 480 in this embodiment.

[0109] It should be noted that P in robotic arm A A This refers to the real-time position of robotic arm A in its own coordinate system, and P in robotic arm B. A Let P be the target position of robotic arm B in its own coordinate system. Both robotic arms have different coordinate systems and coordinates. Other examples include P... B The same applies to others.

[0110] (3) First, the initial resultant force of the end of the robotic arm A is obtained based on the guiding force of the end of the robotic arm A and the interaction force of the end of the robotic arm B. At the same time, the initial resultant force of the end of the robotic arm B is obtained based on the guiding force of the end of the robotic arm B and the interaction force of the end of the robotic arm A. Then, the final resultant force of each end of the two robotic arms is obtained based on their respective initial resultant forces. Then, the final resultant forces of each end of the two robotic arms are constrained according to the motion range boundary model to obtain the auxiliary forces of each end of the two robotic arms.

[0111] The interaction force between the current robotic arm and another robotic arm needs to be initialized before participating in the calculation. Specifically, the interaction force in the corresponding direction is set to 0 depending on the selected operating space mode: if the planar mode is selected, the interaction force in the z-axis direction is set to 0; if the vertical mode is selected, the interaction force in the x-axis direction is set to 0; if the three-dimensional space mode is selected, the magnitude of the interaction force in the three directions remains unchanged.

[0112] Furthermore, the specific steps to obtain the initial resultant force at the end of robotic arm A are as follows: Substitute the guiding force at the end of robotic arm A and the interaction force of robotic arm B into the following formula and perform vector summation to obtain the initial resultant force at the end of robotic arm A.

[0113] F r1A =F gA +F iB

[0114] In the above formula, F r1A F is the initial net force at the end of the current robotic arm A, and is a vector; gA F is the guiding force at the end of the current robotic arm A. iB This is the interaction force received by the current robotic arm A from the end of another robotic arm B.

[0115] The specific steps to obtain the initial resultant force at the end of robotic arm B are as follows: Substitute the guiding force at the end of robotic arm B and the interaction force of robotic arm A into the following formula and perform vector summation to obtain the initial resultant force at the end of robotic arm B.

[0116] F r1B =FgB +F iA

[0117] In the above formula, F r1B F is the initial resultant force at the end of the current robotic arm B, and is a vector; gB F is the guiding force at the end of the current robotic arm B. iA The interaction force received by the current robotic arm B from the end of another robotic arm A.

[0118] Furthermore, the specific steps to obtain the final resultant force of each of the two robotic arms from their initial resultant forces at the ends are as follows: The initial resultant forces F of the two robotic arms at the ends are... r1 Substitute into formula F r2 =min(F r1 ,F max The final resultant force F is obtained. r2 Size,

[0119] In the above formula, F r2 The final resultant force at the end of the robotic arm; F r1 Let F be the initial net force at the end of the robotic arm. r1A or F r1B ;F max This is the maximum force applied to the end effector of the robotic arm. Considering patient safety, in this embodiment, F... max The setting is 30N to prevent secondary harm to the patient.

[0120] Furthermore, based on the boundary model of the motion range, the final resultant force of the end effector of the robotic arm is constrained. The specific steps to obtain the auxiliary force of the end effector are as follows: First, calculate the axial direction components of the interaction force direction and motion trend direction of the end effector on the xyz axes. Then, compare the similarities and differences of the axial direction components on different coordinate axes.

[0121] When the axial component of the interaction force direction is the same as the axial component of the motion trend direction, the final resultant force at the end of the robotic arm along the coordinate axis is substituted into the motion range boundary model formula to obtain the axial component of the auxiliary force at the end of the robotic arm along that coordinate axis. In other words, the axial component of the auxiliary force at the end of the robotic arm along that coordinate axis is reduced, making the auxiliary force at the end of the robotic arm reduced to 0 within the rigid motion range. The motion range boundary model formula is shown below.

[0122]

[0123] In the above formula, P is the current position of the end effector of the robotic arm; D1 is the normal range of motion of the end effector; D2 is the rigid range of motion of the end effector, which is the protection range surrounding the normal range of motion. Within this protection range, the auxiliary force gradually reduces the final resultant force to 0 according to the change in the distance from the real-time position to the boundary; C is a constant, the specific value of which is determined by the interaction force of the robotic arm itself. D1 and D2 are both obtained relative to the robotic arm's own coordinate system.

[0124] When the axial component of the interaction force direction is opposite to the axial component of the motion trend direction, the final resultant force at the end of the robotic arm along the axial component of the coordinate axis is recorded as the axial component of the auxiliary force at the end of the robotic arm on the coordinate axis, ensuring that the robotic arm can be quickly pulled back to the boundary of the motion range after it goes out of bounds and can move normally.

[0125] Finally, the axial component vectors of the auxiliary force at the end of the robotic arm are summed along the x, y, and z axes to obtain the auxiliary force at the end of the robotic arm.

[0126] (4) Substitute the interaction force and auxiliary force of the two robotic arms at their ends into the admittance controller (e.g. Figure 3 The admittance control model formula (shown) yields the desired velocities of the two robotic arms' ends, and then controls the two robotic arms to perform corresponding operations based on their respective desired velocities, achieving a coordinated motion effect. The admittance control model formula is shown below.

[0127]

[0128] In the above formula, M is the inertia matrix; D is the damping matrix; K is the stiffness matrix; F ext The resultant force is the interaction force between the auxiliary force at the end effector of the robotic arm and the robotic arm itself, Δx = x0 - x d , where x d , Let x0 be the robot's desired pose, velocity, and acceleration. This refers to the position, velocity, and acceleration values ​​that the robot theoretically needs to track when the external force is zero.

[0129] Example 2

[0130] This embodiment provides a dual-machine collaborative upper limb rehabilitation robot system, including: a position and interaction force acquisition module, a guiding force determination module, an auxiliary force determination module for the constrained system, and a robotic arm execution module; wherein, the guiding force determination module further includes a guiding force direction determination unit and a guiding force magnitude determination unit.

[0131] Furthermore, the position and interaction force acquisition module is used to acquire the real-time position and interaction force of each end effector of the two robotic arms in its own coordinate system. It should be noted that the real-time position and interaction force of the end effectors of the robotic arms are calculated relative to their own coordinate system; wherein, the robotic arm's own coordinate system is the robotic arm base coordinate system established with the center point of the robotic arm base as the origin.

[0132] A guiding force determination module is provided, with its input connected to the output of the position and interaction force acquisition module. The guiding force determination module acquires the real-time positions of the two robotic arm ends in their respective coordinate systems, as output by the position and interaction force acquisition module. It then converts the real-time position of robotic arm A in its own coordinate system into coordinate value A relative to the coordinate system of robotic arm B, and records coordinate value A as the target position of robotic arm B. Based on the real-time position and target position of robotic arm B, a guiding force is generated acting on robotic arm B. Simultaneously, the guiding force determination module also converts the real-time position of robotic arm B in its own coordinate system into coordinate value B relative to the coordinate system of robotic arm A, records coordinate value B as the target position of robotic arm A, and generates a guiding force acting on robotic arm A based on the real-time position and target position of robotic arm A. Before assigning coordinate values ​​A or B as the target position of the robotic arm's end effector, constraints need to be applied to A or B based on abstract motion boundaries to obtain constrained coordinate values ​​A or B. The specific steps of the constraint process are as follows: the x-axis coordinates of A or B are restricted to the range [-m, +m], and the y-axis and z-axis coordinates are restricted to the range [0, m], where m is a constant. Furthermore, coordinate scaling transformations are performed on the constrained coordinate values ​​A or B to ensure the relative accuracy of the positions of the two robotic arms when they move within different motion ranges.

[0133] The guiding force direction determination unit is used to convert the real-time position of the end of the robotic arm A in its own coordinate system into a coordinate value A relative to the coordinate system of the robotic arm B, and record the coordinate value A as the target position of the end of the robotic arm B. Then, in the same coordinate system, the direction of the guiding force of the end of the robotic arm B is determined based on the real-time position of the end of the robotic arm B and the target position of the end of the robotic arm B. Simultaneously, the guiding force direction determination unit is also used to convert the real-time position of the end of the robotic arm B in its own coordinate system into a coordinate value B relative to the coordinate system of the robotic arm A, and record the coordinate value B as the target position of the end of the robotic arm A. Then, in the same coordinate system, the direction of the guiding force of the end of the robotic arm A is determined based on the real-time position of the end of the robotic arm A and the target position of the end of the robotic arm A.

[0134] The guiding force magnitude determination unit is used to generate the magnitude of the guiding force at the end of the robotic arm; the step S10 of the guiding force magnitude determination unit determining the magnitude of the guiding force at the end of the robotic arm specifically includes:

[0135] S11, obtain the real-time position of the robotic arm's end effector in its own coordinate system and the target position from the direction determination unit of the guiding force, and obtain the distance between the two according to the following formula.

[0136]

[0137] In the above formula, For the current position of robotic arm A from its own real-time position P A To the target location P B The distance between them, where P A =[x A y A z A ] T x A y A z A The current real-time position P of robotic arm A is... A The three-dimensional coordinates of P B =[x B y B z B ] T x B y B z B The target position P of the current robotic arm A. B The three-dimensional coordinates;

[0138] S12, Substitute the distance obtained from S11 into the following formula to obtain the magnitude of the guiding force at the end of the robotic arm.

[0139]

[0140] In the above formula, F gA This represents the magnitude of the guiding force of the current robotic arm A. For the current position of robotic arm A from its own real-time position P A Pointing to the target location P B The vector; r is the threshold of the unguided force range; S A Let be the virtual stiffness of the current robotic arm A.

[0141] The constrained system's auxiliary force determination module has its input terminals connected to the output terminals of the position and interaction force acquisition module and the guiding force determination module, respectively. This module acquires the interaction forces of the two robotic arm ends output by the position and interaction force acquisition module and the guiding forces of the two robotic arm ends output by the guiding force determination module. Then, based on the guiding force of robotic arm A and the interaction force of robotic arm B, the final resultant force of robotic arm A is obtained, and simultaneously, based on the guiding force of robotic arm B and the interaction force of robotic arm A, the final resultant force of robotic arm B is obtained. Finally, based on the motion range boundary model, the final resultant forces of the two robotic arm ends are constrained to obtain the auxiliary forces of each robotic arm end. The interaction force between the current robotic arm and another robotic arm needs to be initialized before participating in the calculation. Specifically, the interaction force in the corresponding direction is set to 0 depending on the selected operating space mode: if the planar mode is selected, the interaction force in the z-axis direction is set to 0; if the vertical mode is selected, the interaction force in the x-axis direction is set to 0; if the three-dimensional space mode is selected, the magnitude of the interaction force in the three directions remains unchanged.

[0142] Specifically, step S20, in which the auxiliary force determination module of the constrained system obtains the final resultant force at the end of the robotic arm, is as follows:

[0143] S21, Determine the initial resultant force: Substitute the guiding force at the current end of the robotic arm and the interaction force at the end of the other robotic arm into the following formula and perform vector summation to obtain the initial resultant force at the current end of the robotic arm.

[0144] F r1A =F gA +F iB

[0145] In the above formula, F r1A F is the initial net force at the end of the current robotic arm A, and is a vector; gA F is the guiding force at the end of the current robotic arm A. iB The interaction force received by the current robotic arm A from the end of another robotic arm B;

[0146] S22, Determine the final resultant force: Substitute the initial resultant force of the current robotic arm end effector obtained in S21 into the following formula to obtain the final resultant force of the current robotic arm end effector.

[0147] F r2 =min(F r1 ,F max )

[0148] In the above formula, F r2 F is the final resultant force at the end effector of the current robotic arm. r1 F is the initial net force at the end effector of the robotic arm. maxThe maximum force applied to the current end effector of the robotic arm can be set by the doctor based on the patient's recovery progress.

[0149] Step S30, which involves constraining the final resultant force at the end of the robotic arm based on the motion range boundary model to obtain the auxiliary force at the end of the robotic arm, specifically involves:

[0150] S31, first calculate the axial direction components of the interaction force direction and motion trend direction of the end of the robotic arm on the xyz three axes, and then compare the similarities and differences of the axial direction components on different coordinate axes.

[0151] When the axial component of the interaction force direction is in the same direction as the axial component of the motion trend direction, the axial component of the final resultant force at the end of the robotic arm along that coordinate axis is substituted into the motion range boundary model formula to obtain the axial component of the auxiliary force at the end of the robotic arm along that coordinate axis. The motion range boundary model formula is shown below.

[0152]

[0153] In the above formula, P is the current position of the end effector of the robotic arm; D1 is the normal range of motion of the end effector; D2 is the rigid range of motion of the end effector, which is the protection range surrounding the normal range of motion. Within this protection range, the auxiliary force gradually reduces the final resultant force to 0 according to the change in the distance from the real-time position to the boundary; C is a constant, the specific value of which is determined by the interaction force of the robotic arm itself. D1 and D2 are both obtained relative to the robotic arm's own coordinate system.

[0154] When the axial component of the interaction force direction is opposite to the axial component of the motion trend direction, the axial component of the final resultant force at the end of the robotic arm along the coordinate axis is denoted as the axial component of the auxiliary force at the end of the robotic arm along the coordinate axis.

[0155] S32, sum the axial component vectors of the auxiliary force at the end of the robotic arm along the x, y, and z axes to obtain the auxiliary force at the end of the robotic arm.

[0156] A robotic arm execution module is provided, with its input terminals connected to the output terminals of both the position and interaction force acquisition module and the auxiliary force determination module of the constrained system. The robotic arm execution module acquires the interaction forces of the two robotic arm ends output by the position and interaction force acquisition module and the auxiliary forces of the two robotic arm ends output by the auxiliary force determination module of the constrained system. Then, it substitutes the interaction forces and auxiliary forces of the robotic arm ends themselves into the admittance control model formula of the admittance controller to obtain the desired velocity of the robotic arm ends. The specific admittance control model formula is shown below.

[0157]

[0158] In the above formula, F aA F is the auxiliary force at the end of the current robotic arm A. iA M represents the interaction force at the end effector of robotic arm A. A D is the inertia matrix; A K is the damping matrix; A Here is the stiffness matrix; Δx = x0 - x d , where x d , Let x0 be the robot's desired pose, velocity, and acceleration. This refers to the position, velocity, and acceleration values ​​that the robot theoretically needs to track when the external force is zero.

[0159] Performance testing:

[0160] (I) The Influence of Assistive Force on the Synergistic Effect of a Two-Machine Collaborative Upper Limb Rehabilitation Robot

[0161] To investigate the effect of assistive force on the synergistic effect of a dual-robot collaborative upper limb rehabilitation robot, the following experiments were conducted: Example 1 and Comparative Example 1. Finally, the three-dimensional graphs and deviation graphs of the motion trajectories of the two robotic arms were compared and analyzed. The results are as follows: Figure 4 and Figure 5 As shown.

[0162] Comparative Example 1

[0163] The control method of a dual-machine collaborative upper limb rehabilitation robot is basically the same as that in Example 1, except that in step (3), the guiding forces of the ends of the two robotic arms are constrained directly according to the motion range boundary model to obtain the constrained guiding forces of the ends of the two robotic arms. Specifically:

[0164] First, calculate the axial components of the interaction force direction and motion trend direction of the robotic arm end effector on the xyz axes. Then, compare the similarities and differences in the axial components on different coordinate axes.

[0165] When the axial component of the interaction force direction is in the same direction as the axial component of the motion trend direction, the axial component of the guiding force at the end of the robotic arm along that coordinate axis is substituted into the motion range boundary model formula to obtain the axial component of the constrained guiding force at the end of the robotic arm along that coordinate axis. The motion range boundary model formula is shown below.

[0166]

[0167] In the above formula, P is the current position of the end effector of the robotic arm; D1 is the normal range of motion of the end effector; D2 is the rigid range of motion of the end effector, which is the protection range surrounding the normal range of motion. Within this protection range, the auxiliary force gradually reduces the final resultant force to 0 according to the change in the distance from the real-time position to the boundary; C is a constant, the specific value of which is determined by the interaction force of the robotic arm itself. D1 and D2 are both obtained relative to the robotic arm's own coordinate system.

[0168] When the axial component of the interaction force direction is opposite to the axial component of the motion trend direction, the axial component of the guiding force at the end of the robotic arm along the coordinate axis is recorded as the axial component of the constrained guiding force at the end of the robotic arm on the coordinate axis, ensuring that the robotic arm can be quickly pulled back to the boundary of the motion range after it goes out of bounds and can move normally.

[0169] Finally, the axial component vectors of the constrained guiding force at the end of the robotic arm are summed along the x, y, and z axes to obtain the constrained guiding force at the end of the robotic arm.

[0170] In step (4), the constrained guiding force obtained in step (3) is used to replace the auxiliary force and substituted into the admittance control model formula to obtain the desired velocity.

[0171] Depend on Figure 4 As can be seen, the red solid line and purple dashed line represent the patient's trajectory and the doctor's trajectory in the case of the combined guiding force and interaction force as auxiliary forces, respectively. The blue solid line and yellow dashed line represent the patient's trajectory and the doctor's trajectory in the case of the guiding force alone as an auxiliary force, respectively. It is evident that the auxiliary force obtained under the combined action of the interaction force and the guiding force makes the motion trajectories of the two robotic arms more similar.

[0172] Depend on Figure 5 It can be seen that the desired velocity obtained by the guiding force alone as an auxiliary force results in a trajectory deviation between the patient's and doctor's ends of the robotic arm between 0.012 and 0.014 m. However, the trajectory deviation obtained by combining the interactive force and the auxiliary force is between 0.002 and 0.004 m. This indicates that the deviation is approximately 0.01 m larger in the case of the guiding force acting alone compared to the combined effect of the guiding force and the interactive force. Therefore, with the combined effect of the guiding force and the interactive force, the trajectories of the patient's and doctor's ends are more closely aligned, resulting in better motion coordination and improved training efficiency and rehabilitation outcomes for the patient.

[0173] (II) The Influence of Stiffness Coefficient on the Synergistic Effect of a Two-Machine Collaborative Upper Limb Rehabilitation Robot

[0174] To investigate the effect of stiffness coefficient on the collaborative effect of a dual-arm upper limb rehabilitation robot, the following experiments were conducted: Example 1 and Comparative Examples 2-3. Finally, the deviation diagrams of the motion trajectories of the two robotic arms were compared and analyzed. The results are as follows: Figure 6 As shown.

[0175] Comparative Example 2

[0176] The control method of a dual-machine collaborative upper limb rehabilitation robot is basically the same as that of Embodiment 1, except that in step (2), the virtual stiffness S of the robotic arm is set to 50 in this comparative example.

[0177] Comparative Example 3

[0178] The control method of a dual-machine collaborative upper limb rehabilitation robot is basically the same as that of Embodiment 1, except that in step (2), the virtual stiffness S of the robotic arm is set to 1000 in the formula for calculating the magnitude of the guiding force in this comparative example.

[0179] Depend on Figure 6 It can be seen that the stiffness coefficient has a significant impact on the synchronization of the end-effector movements of the two robotic arms: the smaller the stiffness coefficient, the greater the trajectory deviation; as the stiffness coefficient increases, the trajectory deviation decreases accordingly. However, the stiffness coefficient cannot be increased indefinitely, because an excessively large stiffness coefficient will cause the robotic arm to vibrate, resulting in a large trajectory deviation and thus affecting the motion coordination effect. Therefore, a moderate stiffness coefficient is usually selected.

[0180] Example 3

[0181] A computer-readable storage medium storing a computer program, the computer program including the dual-machine collaborative upper limb rehabilitation robot system described in Embodiment 2 above; or / and the computer program, when executed by a processor, implements any step in the control method of the dual-machine collaborative upper limb rehabilitation robot as described in Embodiment 1 above.

[0182] Example 4

[0183] An electronic device includes a memory and a processor, wherein the memory stores a computer program, the computer program including the dual-machine collaborative upper limb rehabilitation robot system described in Embodiment 2 above; and / or the processor, when executing the computer program, implements any step in the control method of the dual-machine collaborative upper limb rehabilitation robot as described in Embodiment 1 above.

[0184] In summary, this invention effectively overcomes the shortcomings of the prior art and has high industrial applicability. The above embodiments are intended to illustrate the substantive content of this invention, but are not intended to limit the scope of protection of this invention. Those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of this invention without departing from the essence and scope of protection of this invention.

Claims

1. A control method for a dual-machine collaborative upper limb rehabilitation robot, characterized in that, Includes the following steps: (1) Obtain the real-time position and interaction force of the two robotic arms in their own coordinate system; (2) Convert the current real-time position of the end of the robotic arm in its own coordinate system into coordinate values ​​relative to the coordinate system of the other robotic arm, and record the coordinate values ​​as the target position of the end of the other robotic arm. Then generate a guiding force acting on the end of the other robotic arm based on the real-time position of the end of the other robotic arm and the target position of the end of the other robotic arm. (3) The final resultant force of the current robotic arm end is obtained based on the guiding force of the current robotic arm end and the interaction force of the other robotic arm end; then, the final resultant force of each of the two robotic arm ends is constrained according to the motion range boundary model to obtain the auxiliary force of each of the two robotic arm ends. (4) Generate the desired speed of each of the two robotic arms based on their own interaction force and their own auxiliary force, so that the two robotic arms can move in cooperation. The guiding force in step (2) includes magnitude and direction; the specific steps for generating the magnitude of the guiding force are as follows: S11, Obtain the real-time position of the robotic arm's end effector in its own coordinate system and the target position, and calculate the distance between them according to the following formula. In the above formula, For the current position of robotic arm A from its own real-time position P A To the target location P B The distance between them, where P A =[x A y A z A ] T x A y A z A The current real-time position P of robotic arm A is... A The three-dimensional coordinates of P B =[x B y B z B ] T x B y B z B The target position P of the current robotic arm A. B The three-dimensional coordinates; S12, Substitute the distance obtained from S11 into the following formula to obtain the magnitude of the guiding force at the end of the robotic arm. In the above formula, F gA This represents the magnitude of the guiding force of the current robotic arm A. For the current position of robotic arm A from its own real-time position P A Pointing to the target location P B The vector; r is the threshold of the unguided force range; S A Let be the virtual stiffness of the current robotic arm A.

2. The control method for the dual-machine collaborative upper limb rehabilitation robot according to claim 1, characterized in that, The virtual stiffness S is 0-1000.

3. The control method for the dual-machine collaborative upper limb rehabilitation robot according to claim 1 or 2, characterized in that, In step (3), the step of obtaining the final resultant force of the current robotic arm end based on the guiding force of the current robotic arm end and the interaction force of the other robotic arm end is specifically as follows: S21, Determine the initial resultant force: Substitute the guiding force at the current end of the robotic arm and the interaction force at the end of the other robotic arm into the following formula and perform vector summation to obtain the initial resultant force at the current end of the robotic arm. F r1A =F gA +F iB In the above formula, F r1A F is the initial net force at the end of the current robotic arm A, and is a vector; gA F is the guiding force at the end of the current robotic arm A. iB The interaction force received by the current robotic arm A from the end of another robotic arm B; S22, Determine the final resultant force: Substitute the initial resultant force of the current robotic arm end effector obtained in S21 into the following formula to obtain the final resultant force of the current robotic arm end effector. F r2 =min(F r1 ,F max ) In the above formula, F r2 F is the final resultant force at the end effector of the current robotic arm. r1 F is the initial net force at the end effector of the current robotic arm. max This represents the maximum force applied to the current end effector of the robotic arm.

4. The control method for the dual-machine collaborative upper limb rehabilitation robot according to claim 3, characterized in that, The specific steps for obtaining the auxiliary force at the end of the robotic arm by constraining the final resultant force at the end of the robotic arm based on the motion range boundary model are as follows: S31, first calculate the axial direction components of the interaction force direction and motion trend direction of the end of the robotic arm on the xyz three axes, and then compare the similarities and differences of the axial direction components on different coordinate axes. When the axial component of the interaction force direction is in the same direction as the axial component of the motion trend direction, the axial component of the final resultant force at the end of the robotic arm along that coordinate axis is substituted into the motion range boundary model formula to obtain the axial component of the auxiliary force at the end of the robotic arm along that coordinate axis. The motion range boundary model formula is shown below. In the above formula, P is the real-time position of the current end effector of the robotic arm; D1 is the normal range of motion of the current end effector of the robotic arm; D2 is the rigid range of motion of the current end effector of the robotic arm; C is a constant, the specific value of which is determined by the interaction force of the current robotic arm itself; where D1 and D2 are obtained relative to the robotic arm's own coordinate system. When the axial component of the interaction force direction is opposite to the axial component of the motion trend direction, the axial component of the final resultant force at the end of the robotic arm along the coordinate axis is denoted as the axial component of the auxiliary force at the end of the robotic arm along the coordinate axis. S32, sum the axial component vectors of the auxiliary force at the end of the robotic arm along the x, y, and z axes to obtain the auxiliary force at the end of the robotic arm.

5. The control method for the dual-machine collaborative upper limb rehabilitation robot according to claim 4, characterized in that, In step (4), the interaction force and auxiliary force at the end of the robotic arm generate the desired speed of the robotic arm through the admittance controller.

6. The control method for the dual-machine collaborative upper limb rehabilitation robot according to claim 5, characterized in that, In step (4), the step of generating the desired speed of the robotic arm through the interaction force and auxiliary force at the end of the robotic arm via the admittance controller is specifically as follows: substituting the interaction force and auxiliary force of the robotic arm itself into the admittance control model formula of the admittance controller to obtain the desired speed of the robotic arm. The admittance control model formula is specifically shown below. In the above formula, M is the inertia matrix; D is the damping matrix; K is the stiffness matrix; F ext The resultant force is the interaction force between the auxiliary force at the end effector of the robotic arm and the robotic arm itself, Δx = x0 - x d , where x d , Let x0 be the robot's desired pose, velocity, and acceleration. This refers to the position, velocity, and acceleration values ​​that the robot theoretically needs to track when the external force is zero.

7. A computer-readable storage medium storing a computer program thereon, characterized in that, When the computer program is executed by the processor, it implements any step in the control method of the dual-machine collaborative upper limb rehabilitation robot as described in any one of claims 1-6.

8. An electronic device comprising a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program, characterized in that, When the processor executes the computer program, it implements any step in the control method of the dual-machine collaborative upper limb rehabilitation robot as described in any one of claims 1-6.

Images