A dual-robot assisted upper limb unilateral rehabilitation training system and a control method thereof

Abstract

The application discloses a kind of double-robot assisted upper limb unilateral rehabilitation training system and control method thereof, it is related to the technical field of rehabilitation robot, the rehabilitation training system includes system controller, double-robot assembly, base assembly and arm support assembly, base assembly includes base and moving platform;Arm support assembly includes quick release device and support assistive device;Double-robot is connected to base and moving platform by lifting column respectively, and is connected with human upper limb forearm and upper limb upper arm by the support assistive device and quick release device of double-robot end respectively, at this time, the control method of the rehabilitation training system is combined, control double-robot assembly works, to drive human upper limb forearm and upper limb upper arm complete rehabilitation training task in three-dimensional workspace.The application can realize the purpose that two robots safely and reliably assist human upper limb to complete specified rehabilitation training task.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of rehabilitation robots, in particular to a double-robot assisted upper limb unilateral rehabilitation training system and a control method thereof. BACKGROUND

[0002] In recent years, the incidence of stroke is on the rise, and its sequelae are prone to cause motor dysfunction of the upper and lower limbs, which seriously affects the daily life of patients. The rehabilitation program for upper and lower limb motor dysfunction depends on the treatment of rehabilitation physicians, which has many limitations such as shortage of physicians, heavy task, low rehabilitation efficiency, etc. Rehabilitation robots can continuously perform repetitive training tasks, which can free rehabilitation physicians from heavy and repetitive training tasks, and use various sensors embedded in rehabilitation robots to record training data, providing objective basis for rehabilitation physicians to improve and optimize rehabilitation programs.

[0003] According to the structure of the rehabilitation robot, the upper limb rehabilitation robot for stroke patients with hemiplegia is mainly divided into an exoskeleton type upper limb rehabilitation robot and an end guided type upper limb rehabilitation robot. The exoskeleton type upper limb rehabilitation robot is often designed as a typical bionic arm structure, and the shoulder, elbow and wrist joints are mostly arranged according to the distribution characteristics of the corresponding joints of the human upper limb. It can be worn on the upper limb to accurately control each joint of the upper limb to realize the independent movement of a single joint and the composite movement of multiple joints of the upper limb. However, in actual application, the mechanical axis is often required to be aligned or coincided with the shoulder joint axis in real time, which is prone to cause human-machine motion incompatibility problem, and has the problems of complex wearing and high cost.

[0004] The end guided type upper limb rehabilitation robot focuses on the motion trajectory of the end hand, and its sensor is often used only to collect the position and force information of the end hand, and lacks effective feedback on the kinematics and mechanics information of other joints of the upper limb. And this kind of robot is often used to assist patients in training in a certain plane, and cannot well realize the joint motion in three-dimensional space, especially for the shoulder joint with multiple degrees of freedom, the number of degrees of freedom and the range of joint motion are very limited, so that the robot is difficult to simulate the daily behavior motion trajectory of the upper limb and realize complex rehabilitation training content. SUMMARY

[0005] The purpose of the present application is to provide a double-robot assisted upper limb unilateral rehabilitation training system and a control method thereof, to realize the purpose of two robots assisting the human upper limb to complete the designated rehabilitation training task safely and reliably.

[0006] To achieve the above purpose, the present application provides the following scheme:

[0007] The application provides a double-robot-assisted upper-limb unilateral rehabilitation training system, which comprises a system controller, a double-robot assembly, a base assembly and an arm support assembly.

[0008] The double-robot assembly comprises a first robot, a second robot, a first robot controller and a second robot controller; wherein the system controller can control the first robot and the second robot to move according to a preset trajectory through the first robot controller and the second robot controller.

[0009] The base assembly comprises a base and a moving platform arranged on the base; wherein the first robot is connected to the base through a first lifting column, and the second robot is connected to the moving platform through a second lifting column.

[0010] The arm support assembly comprises a first quick-release device, a first support assistive device, a second quick-release device and a second support assistive device; wherein one end of the first quick-release device is mounted to the end of the first robot, and the other end of the first quick-release device is used for quickly releasing and mounting the first support assistive device; one end of the second quick-release device is mounted to the end of the second robot, and the other end of the second quick-release device is used for quickly releasing and mounting the second support assistive device; the first support assistive device is used for supporting the upper arm region of the upper limb; and the second support assistive device is used for supporting the forearm region of the upper limb.

[0011] The application further provides a control method of the double-robot-assisted upper-limb unilateral rehabilitation training system, which comprises the following steps:

[0012] Before the patient's upper limb is worn to the first robot and the second robot, the patient's upper limb is driven by external power to move according to an expected trajectory, the motion information recorded by the first inertia sensor, the second inertia sensor and the third inertia sensor located on the upper limb is acquired, and the pose transformation matrix from the upper arm coordinate system of the upper limb to the forearm coordinate system of the upper limb, the pose transformation matrix from the upper arm coordinate system of the upper limb to the shoulder joint coordinate system of the upper limb and the pose transformation matrix from the shoulder joint coordinate system of the upper limb to the world coordinate system are determined according to the recorded motion information; wherein the world coordinate system is the base coordinate system of the first robot.

[0013] The pose transformation matrix from the base coordinate system of the second robot to the base coordinate system of the first robot is determined through the position information of the first robot, the position information of the second robot and a calibration method.

[0014] The single robot kinematic chain equation is solved by decoupling the closed kinematic chain, and the pose transformation matrix from the end coordinate system of the first robot to the base coordinate system of the first robot and the pose transformation matrix from the end coordinate system of the second robot to the base coordinate system of the second robot are determined according to the single robot kinematic chain equation, the pose transformation matrix from the base coordinate system of the second robot to the base coordinate system of the first robot, and a constant matrix; the constant matrix includes the pose transformation matrix from the first support assistive device coordinate system to the end coordinate system of the first robot, the pose transformation matrix from the upper arm coordinate system to the end coordinate system of the first robot, the pose transformation matrix from the second support assistive device coordinate system to the end coordinate system of the second robot, and the pose transformation matrix from the forearm coordinate system to the second support assistive device coordinate system.

[0015] According to the training mode, the training parameters and the pose transformation matrix set, the smooth motion trajectory of the first robot in the joint space and the smooth motion trajectory of the second robot in the joint space are determined; the pose transformation matrix set includes the pose transformation matrix from the upper arm coordinate system to the forearm coordinate system, the pose transformation matrix from the upper arm coordinate system to the shoulder joint coordinate system, the pose transformation matrix from the shoulder joint coordinate system to the world coordinate system, the pose transformation matrix from the end coordinate system of the first robot to the base coordinate system of the first robot, and the pose transformation matrix from the end coordinate system of the second robot to the base coordinate system of the second robot.

[0016] The smooth motion trajectory of the first robot in the joint space is sent to the first robot, and the smooth motion trajectory of the second robot in the joint space is sent to the second robot, so that the first robot and the second robot move according to the received smooth motion trajectory, to realize the coordinated operation control of the dual robots.

[0017] According to the specific embodiments of the present application, the following technical effects are provided:

[0018] 1. The present application uses dual robots to act on the forearm and the upper arm of the upper limb respectively, and has the advantages of exoskeleton robots and end-guided robots, and through the coordinated control of the dual robots, the designated rehabilitation training task of the upper limb of the human body is safely and reliably assisted.

[0019] 2. The double robot and the upper limbs of the human body constitute a complex coupled closed kinematic chain, which brings challenges to solving and planning the cooperative motion trajectory of the double robot. Based on this scene, the application proposes a feasible double robot cooperative work control method, that is, by decoupling the closed kinematic chain to solve the single robot kinematic chain equation, with the help of multiple inertial sensors, the pose transformation matrix from the coordinate system of the end of the first robot to the coordinate system of the base of the first robot and the pose transformation matrix from the coordinate system of the end of the second robot to the coordinate system of the base of the second robot are solved, and then combined with MATLAB tool, the joint space motion trajectory of the double robot is solved, and the cooperative work control of the double robot is realized.

[0020] 3. The control method fully considers the relative position layout relationship between the double robots, the robot and the human body, and the individual differences of different users, so that the trajectory planning of the double robot can be adaptively regulated according to the sitting posture and height of the user. DETAILED DESCRIPTION

[0021] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments. Obviously, the drawings in the following description only constitute some embodiments of the present application, and for those skilled in the art, other drawings can also be obtained from these drawings without creative labor.

[0022] Figure 1 The structure schematic diagram of the double robot assisted unilateral limb rehabilitation training provided for the embodiment of the present application is shown in the figure.

[0023] Figure 2 The structure schematic diagram of the mobile platform provided for the embodiment of the present application is shown in the figure.

[0024] Figure 3 The structure schematic diagram of the quick release device provided for the embodiment of the present application is shown in the figure.

[0025] Figure 4 The support auxiliary installation position schematic diagram provided for the embodiment of the present application is shown in the figure.

[0026] Figure 5 The coordinate system diagram of the double robot assisted upper limb rehabilitation training system provided for the embodiment of the present application is shown in the figure.

[0027] Figure 6 The inertial sensor installation position schematic diagram provided for the embodiment of the present application is shown in the figure.

[0028] Figure 7 The flowchart of the control method of the double robot assisted upper limb unilateral rehabilitation training system provided for the embodiment of the present application is shown in the figure. DETAILED DESCRIPTION

[0030] First Robot-1, Second Robot-2, Base-3, Mobile Platform-4, First Lifting Column-5, Second Lifting Column-6, First Quick-Release Device-7, First Support Auxiliary Fixture-8, Second Quick-Release Device-9, Second Support Auxiliary Fixture-10, Display Screen-11, Support Frame-12, Motor-401, Motor Mounting Bracket-402, Bearing Seat-403, Screw-404, Coupling-405, Support Plate-406 Guide rail-407, slider-408, stop-block-409, limit switch-410, limit baffle-411, flange-701, pressure rod cap-702, pressure rod-703, connecting rod-704, push rod-705, spring-706, pin-shaft-707, shaft retaining ring-708, push rod inner cavity-709, inclined surface-710, first support auxiliary mounting hole-711, first support auxiliary mounting shaft-712. Detailed Implementation

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

[0032] This invention draws on the advantages of existing exoskeleton-type upper limb rehabilitation robots and end-guided upper limb rehabilitation robots to provide a dual-robot assisted unilateral upper limb rehabilitation training system and control method, enabling two robots to safely and reliably assist the human upper limb in completing designated rehabilitation training tasks.

[0033] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0034] like Figure 1 As shown in the figure, the dual-robot assisted unilateral upper limb rehabilitation training system provided in this embodiment mainly includes: a system controller, a dual-robot assembly, a base assembly, and an arm support assembly.

[0035] The dual-robot assembly includes a first robot 1, a second robot 2, a first robot controller, and a second robot controller; wherein, the system controller can control the first robot 1 and the second robot 2 to move along a preset trajectory through the first robot controller and the second robot controller, and simultaneously receive motion information from the first robot 1 and the second robot 2.

[0036] The base assembly comprises a base 3 and a moving platform 4 arranged on the base 3; wherein the first robot 1 is connected to the base 3 through a first lifting column 5, and the second robot 2 is connected to the moving platform 4 through a second lifting column 6.

[0037] The arm support assembly comprises a first quick release device 7, a first support aid 8, a second quick release device 9 and a second support aid 10; wherein one end of the first quick release device 7 is mounted at the end of the first robot 1, and the other end of the first quick release device 7 is used for quick disassembly and installation of the first support aid 8; one end of the second quick release device 9 is mounted at the end of the second robot 2, and the other end of the second quick release device 9 is used for quick disassembly and installation of the second support aid 10, so as to meet the convenient replacement requirement of different support aids; the first support aid 8 is used for supporting the upper arm region of the upper limb; and the second support aid 10 is used for supporting the forearm region of the upper limb.

[0038] Figure 1 The display is used for assisting the right upper limb of the human body to perform rehabilitation training, and similarly, can also assist the left upper limb of the human body; in use, the positions of the ends of the first robot 1 and the second robot 2 can be re-adjusted to the opposite positions of the base 3 in the figure, and the seat can be moved to the opposite side of the base 3 in the figure.

[0039] Preferably, the first robot 1 and the second robot 2 are end-guided upper limb rehabilitation robots.

[0040] In the embodiment, the double robots are connected to the base 3 and the moving platform 4 through the lifting columns respectively, and are connected to the forearm of the upper limb and the upper arm of the upper limb through the support aids and the quick release devices at the ends of the double robots respectively, so as to drive the forearm of the upper limb and the upper arm of the upper limb to complete the rehabilitation training task in the three-dimensional working space. Obviously, the system provided in the embodiment can safely and reliably assist the upper limb of the human body to complete the specified rehabilitation training task through the advantages of the double robots.

[0041] In the embodiment, the system controller is in communication connection with the moving platform 4; the system controller is used for outputting a moving instruction; the moving instruction is used for controlling the moving platform 4 to move, thereby driving the second robot 2 to move, so as to adjust the horizontal distance between the first robot 1 and the second robot 2, and realize the effect of providing a training cooperation working space for patients with different individual differences.

[0042] Specifically, as Figure 2As shown, the moving platform 4 is composed of a motor 401, a motor mounting frame 402, a bearing seat 403, a screw rod 404, a coupling 405, a support plate 406, a guide rail 407, a sliding block 408, a stop block 409, a limit switch 410, and a limit stop plate 411. The motor 401 is mounted on the base 3 through the motor mounting frame 402. The output shaft of the motor 401 is connected with the screw rod 404 through the coupling 405. The screw rod 404 is supported at both ends by the bearing seat 403, and is connected with the support plate 406 through a screw pair. The support plate 406 is mounted on the guide rails 407 on both sides of the support plate 406 through four sliding blocks 408. In order to limit the moving distance of the support plate 406, the limit switches 410 are installed at both sides of the support plate 406, and the limit stop plates 411 are installed at both sides of the bottom of the support plate 406. The limit stop plates 411 move with the support plate 406.

[0043] The system controller transmits the control signal to the motor driver, and the motor driver drives the motor to rotate the screw rod 404 and move the support plate 406. When the limit stop plate 411 approaches and triggers the proximity limit switch 410, a signal to the position can be sent to the system controller. The system controller sends a control instruction to stop the movement to the motor driver, and stops the motor 401 from moving, so as to limit the moving distance of the support plate 406. In addition, the stop blocks 409 made of elastic material at both ends of the guide rail 407 can also ensure that the support plate 406 moves within a limited range.

[0044] In the embodiment, the first quick release device 7 is the same as the second quick release device 9. The first quick release device 7 is taken as an example for description. Figure 3 and Figure 4As shown, the first quick release device 7 includes a flange plate 701, a pressure rod cap 702, a pressure rod 703, a connecting rod 704, a push rod 705, a spring 706, a pin shaft 707, an axle check ring 708, etc. The flange plate 701 is connected to the robot end through a screw, and a first support auxiliary installation hole 711 is opened on the flange plate 701, and three holes are opened around the first support auxiliary installation hole 711, wherein one hole is installed with the pressure rod 703, the pressure rod 703 is installed with the pressure rod cap 702 at the outer end, and is connected with one end of the connecting rod 704 at the inner end; the other two holes are installed with the spring 706 and the push rod 705, wherein one end of the spring 706 acts on the inner wall of the hole of the flange plate 701, and the other end acts on the bottom surface of the inner cavity 709 of the push rod. The connecting rod 704 connects the pressure rod 703 and the push rod 705 through the pin shaft 707 and the axle check ring 708 to form a hinged connection. The outer end of the push rod 705 is designed with an inclined surface 710. When the first support auxiliary is needed to be installed, the first support auxiliary is pressed downward into the first support auxiliary installation hole 711, which will push the push rod 705 to move into the hole and compress the spring 706; when the first support auxiliary is pressed downward to a certain distance, the spring 706 automatically stretches back to realize the locking of the first support auxiliary installation shaft 712, thereby completing the installation of the first support auxiliary; when the first support auxiliary is needed to be disassembled, the pressure rod cap 702 is pushed inward, the pressure rod 703 drives the push rod 705 to move into the hole through the connecting rod 704 and compresses the spring 706, so that the inclined surface 710 at the outer end of the push rod 705 is separated from the first support auxiliary, thereby completing the disassembly of the first support auxiliary.

[0045] In the embodiment, the system further comprises a display screen assembly; the display screen assembly comprises a display screen 11 and a support frame 12. The display screen 11 is installed on the support frame 12 through a revolute pair, and the angle of the display screen 11 can be rotated according to needs; in addition, the display screen 11 is provided with an inclination adjusting function, so as to adapt to the visual field of patients with different body types and different sitting postures.

[0046] The display screen assembly is arranged at a certain position away from the base, and the back of the display screen 11 is electrically connected and communicatively connected with the system controller through a cable. The display screen 11 sends the information input by the patient to the system controller through the communication connection, and receives and displays the information fed back from the system controller.

[0047] A man-machine interface is run on the display screen 11, wherein the man-machine interface mainly comprises a patient personal basic information display module, a training mode selection and training parameter setting module, a rehabilitation training game module, and a feedback data display and patient training evaluation module; the patient personal basic information display module is used for obtaining and displaying the personal information of the patient such as age, gender, hospital number, etc., the training mode selection and training parameter setting module is used for receiving the training mode and training parameters selected by the patient, the rehabilitation training game module is used for guiding the patient to complete the designated rehabilitation training task in a game mode, and the feedback data display and patient training evaluation module is used for displaying the information such as interactive force and motion position sent by the system controller during the training of the patient, and receiving and displaying the evaluation results after the training of the patient.

[0048] In the embodiment, the system further comprises a first inertial sensor, a second inertial sensor, a third inertial sensor and a fourth inertial sensor; the first inertial sensor is arranged at the center of the upper arm; the second inertial sensor is arranged at the center of the upper arm; the third inertial sensor is arranged at the center of the shoulder joint; and the fourth inertial sensor is arranged on the base of the first robot 1; the inertial sensors are used to record the motion information of the upper arm and the motion information of the first robot 1.

[0049] Embodiment two

[0050] The embodiment provides a control method of a double-robot assisted upper limb unilateral rehabilitation training system. Before introducing the control method provided by the embodiment, first, the establishment process of the kinematic coordinate system of the double robot and the human upper limb, the solving process of the single robot motion chain and the solving process of the human upper limb pose are introduced.

[0051] The kinematic coordinate system establishment process of the double robot and the human upper limb is as shown in the figure. Figure 5 As shown in the figure, [R1] and [R2] are the base coordinate system of the first robot and the base coordinate system of the second robot respectively, the world coordinate system [W] coincides with the base coordinate system [R1] of the first robot, [E1] and [T1] are the end coordinate system (or the 6th axis end coordinate system) of the first robot and the first support auxiliary coordinate system respectively, [E2] and [T2] are the end coordinate system of the second robot and the second support auxiliary coordinate system respectively, [P1] and [P2] are the upper arm coordinate system and the forearm coordinate system of the upper limb respectively, Y T X represents the pose transformation matrix from the X coordinate system to the Y coordinate system.

[0052] As shown in the figure. Figure 6As shown, four inertial sensors are placed near the coordinate systems [P1], [P2], [S] and [R1], and are respectively named as the first inertial sensor, the second inertial sensor, the third inertial sensor and the fourth inertial sensor. The first inertial sensor, the second inertial sensor and the third inertial sensor can be fixed on the upper arm center O P2 , the upper arm center O P1 of the upper limb and the shoulder joint axis center O S respectively by cooperating with the binding belt. R1 The fourth inertial sensor is installed on the base of the first robot and coincides with the origin O

[0053] The solving process of the single robot kinematic chain is as follows: by decoupling the coupled closed kinematic chain, the kinematic chain expression of the first robot and the second robot during the specified trajectory motion can be obtained, that is:

[0054] R1 T E1 · E1 T T1 · T1 T P1 = R1 T S · S T P1 (1).

[0055] R1 T R2 · R2 T E2 · E2 T T2 · T2 T P2 · P2 T P1 = R1 T E1 · E1 T T1 · T1 T P1 (2).

[0056] The pose transformation matrix of the end coordinate system of the first robot and the second robot relative to the robot base coordinate system in the double robot cooperative motion can be obtained through formulas (1) and (2).

[0057] R1 T E1 = R1 T S · S T P1 · E1 T T1 · T1 T P1 ​-1 (3).

[0058] R2 T E2 = R1 T R2 ) -1 · R1 T E1 · E1 T T1 · T1 T P1 ·( E2 T T2 · T2 T P2 · P2 T P1 ) -1 (4).

[0059] wherein, R1 T E1 denotes a pose transformation matrix from the end coordinate system of the first robot to the base coordinate system of the first robot; R2 T E2 denotes a pose transformation matrix from the end coordinate system of the second robot to the base coordinate system of the second robot.

[0060] The related parameter explanations involved in the above formulas (1)-(4) are shown in Table 1.

[0061] Table 1: Partial parameter meaning and explanation table

[0062]

[0063] The solving process of the human upper limb pose is as follows: the upper limb pose is solved to obtain a pose transformation matrix from the upper limb upper arm coordinate system to the upper limb forearm coordinate system, a pose transformation matrix from the upper limb upper arm coordinate system to the upper limb shoulder joint coordinate system, and a pose transformation matrix from the upper limb shoulder joint coordinate system to the base coordinate system of the first robot.

[0064] The pose transformation matrix from the upper limb upper arm coordinate system to the upper limb forearm coordinate system is:

[0065]

[0066] is a rotation transformation matrix in the upper limb upper arm coordinate system to the upper limb forearm coordinate system, which can be determined by the quaternion of the inertial sensor:

[0067]

[0068] wherein q1, q2, q3, q4 are real numbers of four elements in the quaternion.

[0069] PP1P2 The origin O of the upper limb / upper arm coordinate system is represented as O. P1 Given the positional relationship of the first and second inertial sensors in the forearm coordinate system, and considering that the first and second inertial sensors are respectively installed at the center of the forearm and the center of the upper arm, then P P1P2 Position coordinates in the upper limb forearm coordinate system (p 1x ,p 1y ,p 1z This can be represented as:

[0070]

[0071] Wherein, l1 and l2 are the forearm length and upper limb forearm length, and their parameters are related to the height h of the human body. For example, the forearm length l1 = 0.146h and l2 = 0.186h.

[0072] Therefore, combining formulas (6) and (7), formula (5) can be expressed as:

[0073]

[0074] The third inertial sensor is fixed to the shoulder joint axis O by a strap. S At this location, the fourth inertial sensor is mounted on the base of the first robot and is aligned with the origin O of the base coordinate system of the first robot. R1 Similarly, by using the quaternions of the inertial sensors and the relative positional relationships, we can obtain the pose transformation matrix from the upper limb upper arm coordinate system to the upper limb shoulder joint coordinate system and the pose transformation matrix from the upper limb shoulder joint coordinate system to the base coordinate system of the first robot:

[0075]

[0076]

[0077] like Figure 7 As shown in the figure, the control method of a dual-robot assisted unilateral upper limb rehabilitation training system provided in this embodiment includes:

[0078] Step 100: Before the patient's upper limb is put on the first robot and the second robot, the patient's upper limb is first driven by external power (e.g., a rehabilitation physician) to move along the desired trajectory. The motion information recorded by the first inertial sensor, the second inertial sensor and the third inertial sensor located on the upper limb is obtained. Based on the recorded motion information, the pose transformation matrix from the upper arm coordinate system to the upper forearm coordinate system, the pose transformation matrix from the upper arm coordinate system to the upper shoulder joint coordinate system and the pose transformation matrix from the upper shoulder joint coordinate system to the base coordinate system of the first robot are determined.

[0079] In the embodiment, the step is to obtain the pose transformation matrix from the upper limb upper arm coordinate system to the upper limb forearm coordinate system, the pose transformation matrix from the upper limb upper arm coordinate system to the upper limb shoulder joint coordinate system, and the pose transformation matrix from the upper limb shoulder joint coordinate system to the base coordinate system of the first robot by formula (5)-(10).

[0080] Step 200: determining the pose transformation matrix from the base coordinate system of the second robot to the base coordinate system of the first robot by the position information of the first robot, the position information of the second robot, and the calibration method.

[0081] Step 300: solving the single robot kinematic chain equation by decoupling the closed kinematic chain, and determining the pose transformation matrix from the end coordinate system of the first robot to the base coordinate system of the first robot and the pose transformation matrix from the end coordinate system of the second robot to the base coordinate system of the second robot according to the single robot kinematic chain equation, the pose transformation matrix from the base coordinate system of the second robot to the base coordinate system of the first robot, and the constant matrix; the constant matrix includes the pose transformation matrix from the first support assistive device coordinate system to the end coordinate system of the first robot, the pose transformation matrix from the upper limb upper arm coordinate system to the end coordinate system of the first robot, the pose transformation matrix from the second support assistive device coordinate system to the end coordinate system of the second robot, and the pose transformation matrix from the upper limb forearm coordinate system to the second support assistive device coordinate system.

[0082] In the embodiment, the step is to combine the relevant constant matrices in Table 1, i.e. E1 T T1 、 T1 T P1 、 E2 T T2 、 T2 T P2 , and bring them into formula (3)-(4), so as to obtain the pose transformation matrix from the end coordinate system of the first robot to the base coordinate system of the first robot and the pose transformation matrix from the end coordinate system of the second robot to the base coordinate system of the second robot.

[0083] Step 400: determining the smooth motion trajectory of the first robot in the joint space and the smooth motion trajectory of the second robot in the joint space according to the training mode, the training parameter, and the pose transformation matrix set; the pose transformation matrix set includes the pose transformation matrix from the upper limb upper arm coordinate system to the upper limb forearm coordinate system, the pose transformation matrix from the upper limb upper arm coordinate system to the upper limb shoulder joint coordinate system, the pose transformation matrix from the upper limb shoulder joint coordinate system to the base coordinate system of the first robot, the pose transformation matrix from the end coordinate system of the first robot to the base coordinate system of the first robot, and the pose transformation matrix from the end coordinate system of the second robot to the base coordinate system of the second robot.

[0084] Step 500: sending the smooth motion trajectory of the first robot in the joint space to the first robot and sending the smooth motion trajectory of the second robot in the joint space to the second robot to control the first robot and the second robot to move according to the received smooth motion trajectory to realize the coordinated operation control of the dual robots.

[0085] In the embodiment, the step 400 and the step 500 specifically include: after the pose transformation matrix from the end coordinate system of the first robot to the base coordinate system of the first robot and the pose transformation matrix from the end coordinate system of the second robot to the base coordinate system of the second robot are obtained in the step 300, a series of discrete angular displacements θ1-θ6 of each joint of the two robots are respectively solved by means of the inverse kinematics equation of the first robot and the second robot with the aid of the MATLAB software tool, and then the smooth motion trajectory of the dual robots in the joint space is planned by using the B-spline curve interpolation; the trajectory planning data in the joint space are sent to the body controllers of the two robots through the system controller, the body controllers send control instructions to the motors of each joint of the robots, the motors are driven to complete the trajectory tracking control of each joint, and thus the coordinated operation control of the dual robots is finally completed to assist the upper limbs to complete the specific rehabilitation training task.

[0086] The embodiments in the specification are described in a progressive manner, and each embodiment focuses on the difference from other embodiments, and the same or similar parts between the embodiments can be referred to each other. For the system disclosed by the embodiments, since it corresponds to the method disclosed by the embodiments, the description is relatively simple, and the related parts can be referred to the method part.

[0087] The principles and implementation manners of the present application are described by using specific examples in the present application, and the above embodiment description is only used to help understand the method of the present application and its core idea; meanwhile, for the general technical personnel in the art, according to the idea of the present application, the specific implementation manner and application range will be changed. In conclusion, the content of the specification should not be understood as the limitation of the present application.

Claims

1. A control method of a rehabilitation training system for assisting upper limbs on both sides of a double robot, characterized by, The method comprises the following steps: Before the upper limb of the patient is worn on the first robot and the second robot, the upper limb of the patient is driven by an external power to move according to an expected trajectory, motion information recorded by a first inertial sensor, a second inertial sensor and a third inertial sensor located on the upper limb of the patient is obtained, and a pose transformation matrix from an upper arm coordinate system of the upper limb to a forearm coordinate system of the upper limb, a pose transformation matrix from the upper arm coordinate system of the upper limb to a shoulder joint coordinate system of the upper limb, and a pose transformation matrix from the shoulder joint coordinate system of the upper limb to a base coordinate system of the first robot are determined according to the recorded motion information; A pose transformation matrix from a base coordinate system of the second robot to the base coordinate system of the first robot is determined through position information of the first robot, position information of the second robot and a calibration method; Single-robot kinematic chain equations are solved by decoupling the closed kinematic chain, and a pose transformation matrix from an end coordinate system of the first robot to the base coordinate system of the first robot and a pose transformation matrix from an end coordinate system of the second robot to the base coordinate system of the second robot are determined according to the single-robot kinematic chain equations, the pose transformation matrix from the base coordinate system of the second robot to the base coordinate system of the first robot and a constant matrix; the constant matrix comprises a pose transformation matrix from a first support assistive device coordinate system to the end coordinate system of the first robot, a pose transformation matrix from the upper arm coordinate system of the upper limb to the end coordinate system of the first robot, a pose transformation matrix from a second support assistive device coordinate system to the end coordinate system of the second robot, and a pose transformation matrix from the forearm coordinate system of the upper limb to the second support assistive device coordinate system; Smooth motion trajectories of the first robot in joint space and smooth motion trajectories of the second robot in joint space are determined according to a training mode, training parameters and a set of pose transformation matrices; the set of pose transformation matrices comprises the pose transformation matrix from the upper arm coordinate system of the upper limb to the forearm coordinate system of the upper limb, the pose transformation matrix from the upper arm coordinate system of the upper limb to the shoulder joint coordinate system of the upper limb, the pose transformation matrix from the shoulder joint coordinate system of the upper limb to the base coordinate system of the first robot, the pose transformation matrix from the end coordinate system of the first robot to the base coordinate system of the first robot, and the pose transformation matrix from the end coordinate system of the second robot to the base coordinate system of the second robot; The smooth motion trajectories of the first robot in joint space are sent to the first robot, and the smooth motion trajectories of the second robot in joint space are sent to the second robot, so as to control the first robot and the second robot to move according to the received smooth motion trajectories, thereby realizing coordinated operation control of the dual robots.

2. A dual-robot assisted rehabilitation training system for upper limbs of one side, for realizing the control method as claimed in claim 1, characterized in that, The system comprises a system controller, a dual robot assembly, a base assembly and an arm support assembly; The dual robot assembly comprises a first robot, a second robot, a first robot controller and a second robot controller; the system controller can control the first robot and the second robot to move according to a preset trajectory through the first robot controller and the second robot controller. ​ The base assembly comprises a base and a moving platform arranged on the base; wherein the first robot is connected to the base through a first lifting column, and the second robot is connected to the moving platform through a second lifting column; The arm support assembly comprises a first quick release device, a first support aid, a second quick release device and a second support aid; wherein one end of the first quick release device is mounted to the end of the first robot, and the other end of the first quick release device is used for quickly releasing and mounting the first support aid; one end of the second quick release device is mounted to the end of the second robot, and the other end of the second quick release device is used for quickly releasing and mounting the second support aid; the first support aid is used for supporting the upper arm region of the upper limb; and the second support aid is used for supporting the forearm region of the upper limb.

3. The dual robot assisted upper limb unilateral rehabilitation training system according to claim 2, characterized in that, The system controller is in communication connection with the moving platform; the system controller is used for outputting a moving instruction; the moving instruction is used for controlling the moving platform to move, thereby driving the second robot to move, so as to adjust the horizontal distance between the first robot and the second robot.

4. The dual robot assisted upper limb unilateral rehabilitation training system according to claim 2 or 3, characterized in that, The moving platform comprises a motor, a motor mounting frame, a shaft coupling, a bearing seat, a screw rod, a support plate, a guide rail, a sliding block, a stop block, a limit switch and a limit baffle; The motor is mounted on the base through the motor mounting frame; the output shaft of the motor is connected with the screw rod through the shaft coupling; the screw rod is supported at both ends by the bearing seat; and the screw rod is connected with the support plate through a screw pair; the support plate is mounted on the guide rails located on both sides of the support plate through a plurality of sliding blocks; the limit switch is mounted at the positions on both sides of the support plate; the limit baffle is mounted at the positions on both sides of the bottom of the support plate, and the limit baffle moves with the support plate.

5. The dual robot assisted upper limb unilateral rehabilitation training system according to claim 2, characterized in that, The first quick release device is the same as the second quick release device; The first quick release device comprises a flange, a pressing rod cap, a pressing rod, a connecting rod, a push rod, a spring, a pin shaft and a shaft baffle; the flange is connected with the end of the first robot through a screw; a first support aid mounting hole is opened in the flange, and three holes are opened around the first support aid mounting hole; one of the holes is mounted with the pressing rod, the outward end of the pressing rod is mounted with the pressing rod cap, and the inward end of the pressing rod is connected with one end of the connecting rod; the other two holes are both mounted with the spring and the push rod; one end of the spring acts on the inner wall of the hole of the flange, and the other end of the spring acts on the bottom surface of the inner cavity of the push rod; the connecting rod connects the pressing rod and the push rod through the pin shaft and the shaft baffle to form a hinged connection, and the outer end of the push rod is designed with an inclined surface.

6. The dual robot assisted upper limb unilateral rehabilitation training system according to claim 2, characterized in that, Further comprising: A display screen assembly; the display screen assembly comprises a display screen and a support frame; the display screen is mounted on the support frame through a rotating pair, and the display screen has an inclination adjusting function.

7. The dual robot assisted upper limb unilateral rehabilitation training system according to claim 6, characterized in that, The display screen is electrically connected and in communication connection with the system controller through a cable.

8. The dual robot assisted upper limb unilateral rehabilitation training system according to claim 7, characterized in that, A man-machine interaction interface is run on the display screen; wherein the man-machine interaction interface comprises a patient personal basic information display module, a training mode selection and training parameter setting module, a rehabilitation training game module, and a feedback data display and patient training evaluation module, wherein the training mode selection and training parameter setting module is used for receiving the training mode and training parameters selected by the patient, the rehabilitation training game module is used for guiding the patient to complete the designated rehabilitation training task through the game mode, and the feedback data display and patient training evaluation module is used for displaying the interactive force information and motion position information sent by the system controller in the patient training process, and receiving the evaluation results after the patient training.

9. The dual robot assisted upper limb unilateral rehabilitation training system according to claim 2, characterized in that, The first robot and the second robot are both end-guided upper limb rehabilitation robots.

10. The dual robot assisted upper limb unilateral rehabilitation training system according to claim 2, characterized in that, Further comprising: A first inertial sensor, a second inertial sensor, a third inertial sensor, and a fourth inertial sensor; wherein the first inertial sensor is used for being fixed at the center of the upper limb forearm; the second inertial sensor is used for being fixed at the center of the upper limb upper arm; the third inertial sensor is used for being fixed at the shaft center of the shoulder joint; and the fourth inertial sensor is installed on the base of the first robot.

Images