Control system of rehabilitation robot

Abstract

The present invention provides a control system for a rehabilitation robot, the method comprising: constructing a motion control diagram according to the real-time collected motion data of the rehabilitation robot and the patient's upper limbs; calculating the displacement data of the end effector according to the motion control diagram and the kinematic model of the rehabilitation robot; establishing the maximum motion space of the end effector according to the kinematic model of the equalization mechanism of the rehabilitation robot, the displacement data of the end effector and the kinematic model; calculating the effective working space of the rehabilitation robot using the end effector and the maximum motion space of the upper limb hemiplegic patient, and controlling the rehabilitation robot using the effective working space. The present invention calculates the displacement data of the end effector using the kinematic model and the motion control diagram, and establishes the mechanical model of the rehabilitation robot using the dynamic model and the displacement data, thereby realizing the control of the rehabilitation robot.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and in particular to a control system of a rehabilitation robot. Background Art

[0002] The upper limbs play an indispensable role in work and life. Due to upper limb movement disorders, patients cannot take care of themselves and need rehabilitation training to achieve the recovery of their limb nerves and muscles.

[0003] For upper limb rehabilitation robots for home use, in order to safely use the upper limb rehabilitation robots for training without the help of a doctor, the product is required to have good safety protection performance, a safe and reasonable range of motion, and a stable and reliable control system.

[0004] Existing upper limb rehabilitation robots all adopt a series connection method, where the joint connecting rods are connected end to end through motors. The front joint needs to bear all the weight of the motor and accessories of the rear joint, and so on. As a result, the entire upper limb rehabilitation robot structure is very bulky and occupies a large area, which seriously affects the application and promotion of the product. Summary of the invention

[0005] Based on this, the purpose of the present invention is to provide a control system for a rehabilitation robot to solve the deficiencies in the above-mentioned technology.

[0006] The present invention provides a control method for a rehabilitation robot, comprising:

[0007] Collecting the motion data of the rehabilitation robot and the patient's upper limbs in real time, and constructing a corresponding motion control diagram according to the motion data;

[0008] Taking the base of the rehabilitation robot as the origin, converting the motion control diagram into a serial mechanism, constructing a kinematic model of the rehabilitation robot, and calculating the displacement data of the end effector of the rehabilitation robot according to the motion control diagram and the kinematic model;

[0009] Establishing a maximum motion space of the end effector of the rehabilitation robot according to the equalized kinematic model of the rehabilitation robot and the displacement data of the end effector of the rehabilitation robot;

[0010] The effective working space of the rehabilitation robot is calculated using the end effector of the rehabilitation robot and the maximum motion space of the upper limb hemiplegic patient, and a dynamic model of the equalization mechanism of the rehabilitation robot is constructed. The rehabilitation robot is controlled using the dynamic model and the effective working space.

[0011] Furthermore, the steps of collecting the motion data of the rehabilitation robot and the patient's upper limbs in real time and constructing a corresponding motion control diagram according to the motion data include:

[0012] Using a data acquisition device to collect data on the upper limb movement of the upper limb hemiplegia patient to identify the movement joint trajectory of the patient's upper limb;

[0013] The motion joint trajectories of the patient's upper limbs are processed using a posture settlement algorithm and a corresponding data processing algorithm to construct a motion control diagram of the patient's upper limbs.

[0014] Furthermore, taking the base of the rehabilitation robot as the origin, converting the motion control diagram into a serial mechanism, constructing a kinematic model of the rehabilitation robot, and calculating the displacement data of the end effector of the rehabilitation robot according to the motion control diagram and the kinematic model include:

[0015] Based on the motion control diagram, the corresponding series mechanism is converted into the corresponding series mechanism, and the kinematic analysis is performed on the series mechanism to construct a parameter table of the rehabilitation robot;

[0016] Taking the base of the rehabilitation robot as the origin, a kinematic model of the rehabilitation robot is constructed, and the displacement data of the end effector of the rehabilitation robot is calculated according to the parameter table of the rehabilitation robot and the kinematic model.

[0017] Furthermore, the steps of constructing a dynamic model of the equalization mechanism of the rehabilitation robot and controlling the rehabilitation robot using the dynamic model and the effective workspace include:

[0018] The corresponding Lagrangian function is obtained according to the dynamic analysis results of the rehabilitation robot. :

[0019] ;

[0020] In the formula, represents the total potential energy of the robot; represents the total kinetic energy of the robot;

[0021] According to the characteristics of the connecting rod, the driving torque of each joint of the rehabilitation robot is solved:

[0022] ;

[0023] In the formula, Represents the angle of freedom of each joint of the rehabilitation robot; Represents the angular velocity of each joint degree of freedom of the rehabilitation robot; Represents the driving torque of each joint of the rehabilitation robot; represents the number of links of the rehabilitation robot;

[0024] Calculating the potential energy and kinetic energy of each joint of the rehabilitation robot according to the working characteristics of the rehabilitation robot to obtain the total potential energy and total kinetic energy of the rehabilitation robot;

[0025] The Lagrangian function of the rehabilitation robot is solved according to the total potential energy and the total kinetic energy, and combined with the effective working space of the rehabilitation robot to calculate the actual workload of the rehabilitation robot.

[0026] The present invention also provides a control system for a rehabilitation robot, comprising:

[0027] A data acquisition module, used to collect the motion data of the rehabilitation robot and the patient's upper limbs in real time, and to construct a corresponding motion control diagram according to the motion data;

[0028] A data calculation module, used to convert the motion control diagram into a serial mechanism with the base of the rehabilitation robot as the origin, and to construct a kinematic model of the rehabilitation robot, and to calculate the displacement data of the end effector of the rehabilitation robot according to the motion control diagram and the kinematic model;

[0029] A space calculation module is used to establish the maximum motion space of the end effector of the rehabilitation robot according to the equalized kinematic model of the rehabilitation robot and the displacement data of the end effector of the rehabilitation robot; a robot control module is used to calculate the effective working space of the rehabilitation robot using the end effector of the rehabilitation robot and the maximum motion space of the upper limb hemiplegic patient, and to construct a dynamic model of the equalized mechanism of the rehabilitation robot, and to control the rehabilitation robot using the dynamic model and the effective working space.

[0030] Furthermore, the data acquisition module includes:

[0031] A data acquisition unit, used for acquiring data on the upper limb movement of the upper limb hemiplegia patient using a data acquisition device, so as to identify the movement joint trajectory of the patient's upper limb;

[0032] The schematic construction unit is used to process the movement joint trajectories of the patient's upper limbs using a posture settlement algorithm and a corresponding data processing algorithm to construct a motion control schematic of the patient's upper limbs.

[0033] Furthermore, the data calculation module includes:

[0034] A parameter table construction unit, used for equalizing the motion control diagram into a corresponding series mechanism, and performing kinematic analysis on the series mechanism to construct a parameter table of the rehabilitation robot;

[0035] A data calculation unit is used to construct a kinematic model of the rehabilitation robot with the base of the rehabilitation robot as the origin, and calculate the displacement data of the end effector of the rehabilitation robot according to the parameter table of the rehabilitation robot and the kinematic model.

[0036] Furthermore, the robot control module is specifically used for:

[0037] The corresponding Lagrangian function is obtained according to the dynamic analysis results of the rehabilitation robot. :

[0038] ;

[0039] In the formula, represents the total potential energy of the robot; represents the total kinetic energy of the robot;

[0040] According to the characteristics of the connecting rod, the driving torque of each joint of the rehabilitation robot is solved:

[0041] ;

[0042] In the formula, Represents the angle of freedom of each joint of the rehabilitation robot; Represents the angular velocity of each joint degree of freedom of the rehabilitation robot; Represents the driving torque of each joint of the rehabilitation robot; represents the number of links of the rehabilitation robot;

[0043] Calculating the potential energy and kinetic energy of each joint of the rehabilitation robot according to the working characteristics of the rehabilitation robot to obtain the total potential energy and total kinetic energy of the rehabilitation robot;

[0044] The Lagrangian function of the rehabilitation robot is solved according to the total potential energy and the total kinetic energy, and combined with the effective working space of the rehabilitation robot to calculate the actual workload of the rehabilitation robot.

[0045] The present invention also provides a readable storage medium on which a computer program is stored, and when the program is executed by a processor, the control method of the rehabilitation robot is implemented.

[0046] The present invention also proposes a computer, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above-mentioned control method of the rehabilitation robot when executing the computer program.

[0047] The control system of the rehabilitation robot in the present invention collects the motion data of the rehabilitation robot and the patient's upper limbs in real time, and constructs a motion control diagram based on the motion data, so as to quickly and accurately obtain the motion joint data of the patient's upper limbs, thereby ensuring the rehabilitation training effect; constructs a kinematic model of the rehabilitation robot, and uses the kinematic model and the motion control diagram to calculate the displacement data of the end effector, solves the effective working space of the rehabilitation robot, constructs a dynamic model of the rehabilitation robot, and uses the dynamic model and the effective working space of the rehabilitation robot to calculate the working force of the end effector, thereby using force analysis to realize the control of the rehabilitation robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 is a flow chart of a control method for a rehabilitation robot in a first embodiment of the present invention;

[0049] Figure 2 for Figure 1 Detailed flow chart of step S101;

[0050] Figure 3 is a sensor distribution diagram of the rehabilitation robot in the first embodiment of the present invention;

[0051] Figure 4 A schematic diagram of the spatial coordinates of the upper limb sensors of an upper limb hemiplegic patient when the rehabilitation robot in the first embodiment of the present invention is working;

[0052] Figure 5 for Figure 1 Detailed flow chart of step S102;

[0053] Figure 6 is a schematic diagram of the mechanism of the rehabilitation robot in the first embodiment of the present invention;

[0054] Figure 7 is a simplified structural diagram of joints 3 and 4 in the rehabilitation robot in the first embodiment of the present invention;

[0055] Figure 8 is a schematic diagram of a dynamic model of a rehabilitation robot in the first embodiment of the present invention;

[0056] Fig. 9 is a structural block diagram of a control system of a rehabilitation robot in a second embodiment of the present invention;

[0057] Fig.10 FIG. 4 is a structural block diagram of a computer in a third embodiment of the present invention.

[0058] The following specific implementation manner will further illustrate the present invention in conjunction with the above-mentioned drawings. DETAILED DESCRIPTION

[0059] In order to facilitate the understanding of the present invention, the present invention will be described more fully below with reference to the relevant drawings. Several embodiments of the present invention are given in the drawings. However, the present invention can be implemented in many different forms and is not limited to the embodiments described herein. On the contrary, the purpose of providing these embodiments is to make the disclosure of the present invention more thorough and comprehensive.

[0060] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which the present invention belongs. The terms used herein in the specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The term "and / or" used herein includes any and all combinations of one or more of the related listed items.

[0061] Embodiment 1

[0062] See also Figure 1 , which shows a control method of a rehabilitation robot in a first embodiment of the present invention, and the method specifically includes steps S101 to S104:

[0063] S101, collecting motion data of the rehabilitation robot and the patient's upper limbs in real time, and constructing a corresponding motion control diagram according to the motion data;

[0064] For further information, see Figure 2 , the step S101 specifically includes steps S1011~S1012:

[0065] S1011, using a data acquisition device to collect data on the upper limb movement of the upper limb hemiplegia patient to identify the movement joint trajectory of the patient's upper limb;

[0066] S1012, using a posture settlement algorithm and a corresponding data processing algorithm to process the movement joint trajectory of the patient's upper limb to construct a motion control diagram of the patient's upper limb.

[0067] In the specific implementation, according to the working characteristics of the upper limbs, its motion control sensor is established. Figure 3As shown. The distance sensor emission point is set at the fixed point of the rehabilitation robot base, and the corresponding capture point is set at the patient's upper limb joint. The first capture point is set on the shoulder joint, the second capture point is set on the elbow joint, the third capture point is set at the wrist and the center point of the circular motion groove of the rehabilitation device, and a nine-axis sensor is set. Through the data collection of these three capture points and the nine-axis sensor, as well as data calculation, the spatial coordinate position of the patient's arm during exercise can be determined, and the movement intention of the upper limb can be predicted.

[0068] Specifically, in order to ensure the effectiveness of the patient's rehabilitation training, the motion capture sensor system can effectively identify its movement joint trajectory and construct the movement intention. Since the nine-axis sensor integrates a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer, it can accurately calculate the spatial posture of the third capture point of the patient's upper limb.

[0069] By using the attitude settlement method, the accelerometer and magnetometer can be used to perform appropriate feedback compensation on the gyroscope, and the optimal solution can be obtained. First, assume that the navigation coordinate system E, the body coordinate system B, and the measurement values ​​of the three axes of the accelerometer are , the three axis measurements of the gyroscope are , the measurements of the three axes of the magnetometer are By converting the attitude angle and quaternion, the initialization quaternion of the upper limb attitude can be calculated according to the attitude, acceleration and magnetometer in the sensor, and then:

[0070] ;

[0071] ;

[0072] ;

[0073] ;

[0074] In the formula, The scalar part of the quaternion representing the upper limb posture; , , The vector part of the quaternion representing the upper limb posture; Indicates the heading angle of the upper limb posture; Indicates the pitch angle of the upper limb posture; Indicates the rolling angle of the upper limb posture;

[0075] According to the motion characteristics of the upper limbs, the acceleration of the motion capture sensor is corrected and calculated, and the rotation matrix can be used to transfer the earth vector to the machine system to obtain The quaternion represents the rotation matrix from the navigation system to the aircraft system:

[0076] ;

[0077] ;

[0078] In the formula, Represents the coordinate rotation matrix; Indicates the three-axis motion speed;

[0079] Will Taking the vector product with the actual accelerometer output, we have

[0080] ;

[0081] In the formula, Represents the vector product of the three-axis measurement value of the accelerometer and the velocity; Represents the three-axis measurement value of the accelerometer;

[0082] According to the motion characteristics of the upper limbs, the magnetometer of the motion capture sensor is corrected and calculated. Transfer to the Department of Geography, the complementary filter algorithm is used to use the magnetometer value for the rotation matrix Switch to a geographic coordinate system.

[0083] ;

[0084] In the formula, Indicates the measured value after conversion; Represents the measurement values ​​of the three axes of the magnetometer;

[0085] Through the rotation matrix The converted magnetometer value is , then the calculation formula for transferring the magnetometer value to the machine system is:

[0086] ;

[0087] ;

[0088] In the formula, Indicates the magnetometer value of the machine system;

[0089] Then we can solve its vector product formula as

[0090] ;

[0091] In the formula, Represents the vector product of the three-axis measurement value of the magnetometer and the angular velocity; Represents the three-axis measurement value of angular velocity;

[0092] According to the calculation, the output value of the gyroscope can be corrected, and then the measured value of the sensor can be corrected and adjusted, and then:

[0093] ;

[0094] In the formula, Indicates the corrected output value of the sensor; represents the proportionality coefficient; represents the integral coefficient;

[0095] The gyroscope's measurements can be corrected:

[0096] ;

[0097] In the formula, represents the derivative after gyro correction; Represents the three-axis measurement values ​​of the gyroscope;

[0098] By correcting and analyzing the data collected by the motion capture sensor, the corrected angular velocity and the quaternion can be obtained to calculate the derivative of the quaternion.

[0099] ;

[0100] In the formula, , , The derivative of the quaternion representing the upper limb posture;

[0101] By integrating the quaternion derivative, the new quaternion is:

[0102] ;

[0103] The corresponding Euler angle value can be calculated based on the quaternion:

[0104] ;

[0105] ;

[0106] ;

[0107] In the formula, Indicates the heading angle of the upper limb posture; Indicates the pitch angle of the upper limb posture; Indicates the rolling angle of the upper limb posture;

[0108] According to the spatial posture of the third capture point, combined with the measurement value of the distance measurement sensor, the spatial coordinates of the three motion capture points relative to the origin are solved. Then the spatial motion point of the patient's upper limb can be solved.

[0109] like Figure 4 As shown in the figure, a distance measurement sensor is set at point O of the fixed base of the rehabilitation robot, and capture points are designed at the center of the shoulder, elbow, and wrist of the upper limb hemiplegic patient to collect the distance values ​​of OM, ON, and OP. , ; The nine-axis sensor at the wrist center capture point can measure the spatial direction angle of the forearm ( ); MN and NP, the limb lengths of patients with upper limb hemiplegia, are known;

[0110] 1. According to the collection distance OM, ON length , we get two centered at the origin O, is the concentric sphere with radius;

[0111] 2. According to the direction of the linear NP vector measured by the nine-axis sensor at point P and the NP length of the limb of the upper limb hemiplegic patient, the coordinates of points N and P can be solved at the same time.

[0112] 3. According to the collection distance OM length , with the origin O as the center, is a sphere with radius

[0113] 4. Based on the coordinates of point N solved above and the limb length MN of the patient with upper limb hemiplegia, the coordinates of point M can be solved.

[0114] According to the above steps, the spatial coordinate positions of the upper limb positioning points M, N, and P of a patient with upper limb hemiplegia can be calculated.

[0115] S102, taking the base of the rehabilitation robot as the origin, converting the motion control diagram into a serial mechanism, constructing a kinematic model of the rehabilitation robot, and calculating the displacement data of the end effector of the rehabilitation robot according to the motion control diagram and the kinematic model;

[0116] For further information, see Figure 5 , the step S102 specifically includes steps S1021~S1022:

[0117] S1021, converting the motion control diagram into a corresponding series mechanism, and performing kinematic analysis on the series mechanism to construct a parameter table of the rehabilitation robot;

[0118] S1022, constructing a kinematic model of the rehabilitation robot with the base of the rehabilitation robot as the origin, and calculating displacement data of the end effector of the rehabilitation robot according to the parameter table of the rehabilitation robot and the kinematic model.

[0119] In the specific implementation, the upper limb rehabilitation robot cannot solve the motion control problem in complex human-machine interaction during use. The present invention studies the control method and system of the four-degree-of-freedom serial-parallel upper limb rehabilitation robot. According to the structural characteristics of the four-degree-of-freedom serial-parallel upper limb rehabilitation robot, its mechanism diagram is as follows: Figure 6 to Figure 7 shown.

[0120] Since the movement of the serial-parallel structure link BE of the rehabilitation robot is affected by both the link AB and the link AD, and if the link AB is immobile, the movement of the link AD will drive the movement of the link BC, thereby affecting the spatial position of point E. And vice versa. In order to better analyze the kinematics and dynamics of the rehabilitation robot, the structural relationship of this serial-parallel coupling is equivalent to a single serial structure. In the equivalent process, a virtual joint 4' is constructed as needed.

[0121] In order to establish the kinematic equation of the four-degree-of-freedom serial-parallel upper limb rehabilitation robot, the parallel mechanism formed by the joints 3, 4, 5, and 6 on the left side is equalized to the joints 3, 4, and 4' on the right side.

[0122] Given the rod lengths AB, BC, CD, AD, and angles a and b, the rotation angle function relationship of the equalized joint 4 can be solved based on the rotation angle values ​​a and b of the active links AB and AD of the serial-parallel robot.

[0123] ;

[0124] ;

[0125] ;

[0126] ;

[0127] ;

[0128] ;

[0129] In the formula, represents the angle between connecting rod AB and connecting rod AD; b represents the angle between connecting rod AB and the vertical direction; a represents the angle between connecting rod AD and the vertical direction; represents the angle between connecting rod AD and virtual connecting rod BD; represents the angle between connecting rod AC and virtual connecting rod AD; C represents the angle between connecting rod AB and connecting rod BE;

[0130] According to the equalized schematic diagram of the four-degree-of-freedom series-parallel upper limb rehabilitation robot mechanism, a parameter table of the robot can be established, as shown in Table 1 below.

[0131] Table 1 DH parameters of the four-DOF serial-parallel robot

[0132]

[0133] In the calculation process of the four-degree-of-freedom serial-parallel upper limb rehabilitation robot, the coordinate systems are interconnected through the coordinate change matrix, and the adjacent connecting rods are connected. i Relative connecting rod i The position of -1 can be transformed by three coordinates using the matrix express:

[0134] ;

[0135] Then we can solve it and get:

[0136] ;

[0137] ;

[0138] ;

[0139] ;

[0140] After calculation and solution, the displacement equation of point E of the end effector of the upper limb rehabilitation robot can be obtained as follows:

[0141] ;

[0142] In the formula, express E The coordinate value of the point in the x direction; express E Point y Coordinate values ​​in direction; express E Point z Coordinate value in direction.

[0143] Solve the coordinates of point E at the moment when the rehabilitation robot moves: ( , , ) and joint motor angle ( , , ). It can be expressed as:

[0144]

[0145] S103, establishing a maximum motion space of the end effector of the rehabilitation robot according to the equalized kinematic model of the rehabilitation robot and the displacement data of the end effector of the rehabilitation robot;

[0146] S104, using the end effector of the rehabilitation robot and the maximum motion space of the upper limb hemiplegic patient to calculate the effective working space of the rehabilitation robot, and constructing a dynamic model of the equalization mechanism of the rehabilitation robot, and using the dynamic model and the effective working space to control the rehabilitation robot.

[0147] Furthermore, the step S104 specifically includes steps S1041 to S1044:

[0148] S1041, obtaining a corresponding Lagrangian function according to the dynamic analysis result of the rehabilitation robot :

[0149] ;

[0150] In the formula, represents the total potential energy of the robot; represents the total kinetic energy of the robot;

[0151] S1042, solving the driving torque of each joint of the rehabilitation robot according to the characteristics of the connecting rod:

[0152] ;

[0153] In the formula, Represents the angle of freedom of each joint of the rehabilitation robot; Represents the angular velocity of each joint degree of freedom of the rehabilitation robot; Represents the driving torque of each joint of the rehabilitation robot; represents the number of links of the rehabilitation robot;

[0154] S1043, calculating the potential energy and kinetic energy value of each joint of the rehabilitation robot according to the working characteristics of the rehabilitation robot to obtain the total potential energy and total kinetic energy of the rehabilitation robot;

[0155] S1044, solving the Lagrangian function of the rehabilitation robot according to the total potential energy and the total kinetic energy, and combining the effective working space of the rehabilitation robot to calculate the actual workload of the rehabilitation robot.

[0156] For specific implementation, please refer to Figure 8 Based on the Lagrange equation method, the dynamic model of the four-degree-of-freedom serial-parallel upper limb rehabilitation robot can be established. According to the dynamic analysis of the four-degree-of-freedom serial-parallel upper limb rehabilitation robot, its Lagrangian function can be obtained. The calculation formula is:

[0157] ;

[0158] In the formula, represents the total potential energy of the robot; represents the total kinetic energy of the robot;

[0159] According to the characteristics of the connecting rod, the driving torque of each joint of the rehabilitation robot can be solved as:

[0160] ;

[0161] Where: Represents the angle of freedom of each joint of the robot; Represents the angular velocity of each joint degree of freedom of the robot; represents the driving torque of each joint; Indicates the number of robot links.

[0162] According to the working characteristics of the rehabilitation robot, the potential energy of each joint can be calculated, and the potential energy of connecting rod 1 is:

[0163] ;

[0164] The potential energy of connecting rod 2 is:

[0165] ;

[0166] The potential energy of connecting rod 3 is:

[0167] ;

[0168] The potential energy of connecting rod 4 is:

[0169] ;

[0170] Then the total potential energy of the rehabilitation robot can be solved as:

[0171]

[0172] In the formula, represents the potential energy of connecting rod 1; represents the mass of connecting rod 1; Represents the distance between the center of mass of connecting rod 1 and the coordinate origin; represents the potential energy of connecting rod 2; represents the mass of connecting rod 2; It represents the distance between the center of mass of link 2 and the hinge point of links 1 and 2; represents the potential energy of connecting rod 3; represents the mass of connecting rod 3; It represents the distance between the center of mass of link 3 and the hinge point of links 2 and 3; represents the potential energy of connecting rod 4; represents the mass of connecting rod 4; It represents the distance between the center of mass of link 4 and the hinge point of links 3 and 4; represents the total potential energy of the robot;

[0173] Similarly, the kinetic energy of each connecting rod of the rehabilitation robot is solved. In the dynamic calculation process, the kinetic energy of its connecting rod is divided into horizontal direction V and direction of rotation E There are two types. Then we can solve

[0174] , , , ;

[0175] In the formula, represents the angular velocity of connecting rod 1; Indicates the rotation angle of connecting rod 1; represents the angular velocity of connecting rod 2; represents the rotation angle of connecting rod 2; represents the angular velocity of connecting rod 3; represents the rotation angle of connecting rod 3; represents the angular velocity of connecting rod 4; represents the rotation angle of connecting rod 4;

[0176] The center of mass of connecting rod 1 is in Cartesian space The velocity of the center of mass in three directions is:

[0177] ;

[0178] In the formula, The center of mass of connecting rod 1 is x Speed ​​in direction; The center of mass of connecting rod 1 is y Speed ​​in direction; The center of mass of connecting rod 1 is z Speed ​​in direction;

[0179] According to the horizontal kinetic energy formula And the kinetic energy formula for the direction of rotation , then the kinetic energy of connecting rod 1 can be solved for:

[0180] ;

[0181] in, represents the moment of inertia of connecting rod 1.

[0182] The kinetic energy of connecting rod 2 can be solved for:

[0183] ;

[0184] Similarly, the kinetic energy of connecting rod 3 can be calculated for:

[0185] ;

[0186] The kinetic energy of connecting rod 4 can be calculated for:

[0187] ;

[0188] Then the total kinetic energy K of the upper limb rehabilitation robot can be solved as:

[0189] ;

[0190] In the formula, represents the kinetic energy of connecting rod 1; represents the horizontal kinetic energy of connecting rod 1; It represents the kinetic energy of the connecting rod 1 in the rotation direction; represents the mass of connecting rod 1; Indicates the movement speed of connecting rod 1; represents the moment of inertia of connecting rod 1; represents the rotational angular velocity of connecting rod 1; represents the angular acceleration of connecting rod 1; Indicates the rotation angle of connecting rod 1; represents the kinetic energy of connecting rod 2; represents the horizontal kinetic energy of connecting rod 2; represents the kinetic energy of the connecting rod 2 in the rotation direction; represents the mass of connecting rod 2; Indicates the movement speed of connecting rod 2; represents the moment of inertia of connecting rod 2; represents the rotational angular velocity of connecting rod 2; represents the angular acceleration of connecting rod 2; represents the rotation angle of connecting rod 2; represents the kinetic energy of connecting rod 3; represents the horizontal kinetic energy of connecting rod 3; represents the kinetic energy of the connecting rod 3 in the rotation direction; represents the mass of connecting rod 3; Indicates the movement speed of connecting rod 3; represents the moment of inertia of connecting rod 3; represents the rotational angular velocity of connecting rod 3; represents the angular acceleration of connecting rod 3; represents the rotation angle of connecting rod 3; represents the kinetic energy of connecting rod 4; represents the horizontal kinetic energy of connecting rod 4; It represents the kinetic energy of the connecting rod 4 in the rotation direction; represents the mass of connecting rod 4; Indicates the movement speed of connecting rod 4; represents the moment of inertia of connecting rod 4; represents the rotational angular velocity of the connecting rod 4; represents the angular acceleration of connecting rod 4; represents the rotation angle of connecting rod 4; represents the total kinetic energy of the robot;

[0191] According to the total potential energy P and total kinetic energy K of the upper limb rehabilitation robot, the Lagrangian function of the system can be solved. By taking the partial derivatives of its various degrees of freedom angles and angular velocities, the rotation angles and acceleration values ​​of each joint of the rehabilitation robot can be solved.

[0192] Similarly, the force on the motor at position point E at time t can be solved and joint motor angle ( , , )’s functional relationship.

[0193]

[0194] According to the above-mentioned equivalent dynamics model, the relationship between the control motion force and coordinates of the robot at any point in space can be solved, which is equivalent to the force relationship of the rehabilitation robot when the patient is at that point.

[0195] Combined with the kinematic calculation results, the coordinates of the end point E of the rehabilitation robot at time t can be solved as ( , , ), end force The relationship between the joint force and the coordinate position of point E can be established as:

[0196]

[0197] by Figure 4 The wrist point P of the upper limb hemiplegia patient is taken as the target point, and the coordinates are marked as , which is equivalent to the spatial position of point E calculated by the rehabilitation robot in series is the actual point, and the coordinates are marked as , there will be a certain deviation between point E and point P during the working process.

[0198] Similarly, according to the spatial direction vector formed by point E and point P at time t , and their distance , determine the movement direction of the rehabilitation robot at time t. Then the movement displacement value of the rehabilitation robot is:

[0199]

[0200]

[0201]

[0202] In order to improve the smoothness of the movement of the rehabilitation robot end, its range of motion is limited to determine whether the above calculated values ​​meet the requirements. , , If the boundary value is exceeded, the value of the adjacent endpoint will be used.

[0203] In summary, the control method of the rehabilitation robot in the above-mentioned embodiments of the present invention collects the motion data of the rehabilitation robot and the patient's upper limbs in real time, and constructs a motion control diagram based on the motion data, so as to quickly and accurately obtain the motion joint data of the patient's upper limbs, thereby ensuring the rehabilitation training effect; constructs a kinematic model of the rehabilitation robot, and uses the kinematic model and the motion control diagram to calculate the displacement data of the end effector, solves the effective working space of the rehabilitation robot, constructs a dynamic model of the rehabilitation robot, and uses the dynamic model and the effective working space of the rehabilitation robot to calculate the working force of the end effector, thereby using force analysis to realize the control of the rehabilitation robot.

[0204] Embodiment 2

[0205] Another aspect of the present invention is to provide a control system for a rehabilitation robot. Fig. 9 , which shows a control system of a rehabilitation robot in a second embodiment of the present invention, the system comprises:

[0206] A data acquisition module 11 is used to collect the motion data of the rehabilitation robot and the patient's upper limbs in real time, and to construct a corresponding motion control diagram according to the motion data;

[0207] Furthermore, the data acquisition module 11 includes:

[0208] A data acquisition unit, used for acquiring data on the upper limb movement of the upper limb hemiplegia patient using a data acquisition device, so as to identify the movement joint trajectory of the patient's upper limb;

[0209] The schematic construction unit is used to process the movement joint trajectories of the patient's upper limbs using a posture settlement algorithm and a corresponding data processing algorithm to construct a motion control schematic of the patient's upper limbs.

[0210] A data calculation module 12, used to convert the motion control diagram into a serial mechanism with the base of the rehabilitation robot as the origin, and to construct a kinematic model of the rehabilitation robot, and to calculate the displacement data of the end effector of the rehabilitation robot according to the motion control diagram and the kinematic model;

[0211] Furthermore, the data calculation module 12 includes:

[0212] A parameter table construction unit, used for equalizing the motion control diagram into a corresponding series mechanism, and performing kinematic analysis on the series mechanism to construct a parameter table of the rehabilitation robot;

[0213] A data calculation unit is used to construct a kinematic model of the rehabilitation robot with the base of the rehabilitation robot as the origin, and calculate the displacement data of the end effector of the rehabilitation robot according to the parameter table of the rehabilitation robot and the kinematic model.

[0214] A space calculation module 13, used to establish a maximum motion space of the end effector of the rehabilitation robot according to the equalized kinematic model of the rehabilitation robot and the displacement data of the end effector of the rehabilitation robot;

[0215] The robot control module 14 is used to calculate the effective working space of the rehabilitation robot using the end effector of the rehabilitation robot and the maximum motion space of the upper limb hemiplegic patient, and to construct a dynamic model of the equalization mechanism of the rehabilitation robot, and to control the rehabilitation robot using the dynamic model and the effective working space.

[0216] Furthermore, the robot control module 14 is specifically used for:

[0217] The corresponding Lagrangian function is obtained according to the dynamic analysis results of the rehabilitation robot. :

[0218] ;

[0219] In the formula, represents the total potential energy of the robot; represents the total kinetic energy of the robot;

[0220] According to the characteristics of the connecting rod, the driving torque of each joint of the rehabilitation robot is solved

[0221] ;

[0222] In the formula, Represents the angle of freedom of each joint of the rehabilitation robot; Represents the angular velocity of each joint degree of freedom of the rehabilitation robot; Represents the driving torque of each joint of the rehabilitation robot; represents the number of links of the rehabilitation robot;

[0223] Calculating the potential energy and kinetic energy of each joint of the rehabilitation robot according to the working characteristics of the rehabilitation robot to obtain the total potential energy and total kinetic energy of the rehabilitation robot;

[0224] The Lagrangian function of the rehabilitation robot is solved according to the total potential energy and the total kinetic energy, and combined with the effective working space of the rehabilitation robot to calculate the actual workload of the rehabilitation robot.

[0225] The functions or operation steps implemented when the above modules and units are executed are generally the same as those in the above method embodiments, and will not be repeated here.

[0226] The control system of the rehabilitation robot provided in the embodiment of the present invention has the same implementation principle and technical effects as those of the aforementioned method embodiment. For the sake of brief description, for matters not mentioned in the system embodiment, reference may be made to the corresponding contents in the aforementioned method embodiment.

[0227] Embodiment 3

[0228] The present invention also provides a computer, see Fig.10 , shown is a computer in the third embodiment of the present invention, including a memory 10, a processor 20, and a computer program 30 stored in the memory 10 and executable on the processor 20, and when the processor 20 executes the computer program 30, the control method of the rehabilitation robot mentioned above is implemented.

[0229] The memory 10 includes at least one type of readable storage medium, which includes a flash memory, a hard disk, a multimedia card, a card-type memory (e.g., an SD or DX memory, etc.), a magnetic memory, a magnetic disk, an optical disk, etc. In some embodiments, the memory 10 may be an internal storage unit of a computer, such as a hard disk of the computer. In other embodiments, the memory 10 may also be an external storage device, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card, etc. Further, the memory 10 may also include both an internal storage unit of the computer and an external storage device. The memory 10 may be used not only to store application software and various types of data installed in the computer, but also to temporarily store data that has been output or is to be output.

[0230] Among them, in some embodiments, the processor 20 can be an electronic control unit (Electronic Control Unit, abbreviated as ECU, also known as a vehicle computer), a central processing unit (Central Processing Unit, CPU), a controller, a microcontroller, a microprocessor or other data processing chip, used to run the program code stored in the memory 10 or process data, such as executing access restriction programs, etc.

[0231] It should be pointed out that Fig.10 The structure shown does not constitute a limitation on the computer. In other embodiments, the computer may include fewer or more components than shown in the figure, or combine certain components, or arrange the components differently.

[0232] The embodiment of the present invention further provides a readable storage medium having a computer program stored thereon, and when the program is executed by a processor, the control method of the rehabilitation robot as described above is implemented.

[0233] Those skilled in the art will appreciate that the logic and / or steps represented in the flowchart or otherwise described herein, for example, may be considered as an ordered list of executable instructions for implementing logical functions, and may be specifically implemented in any computer-readable medium for use by an instruction execution system, device or apparatus (such as a computer-based system, a system including a processor, or other system that can fetch instructions from an instruction execution system, device or apparatus and execute instructions), or in conjunction with such instruction execution systems, devices or apparatuses. For purposes of this specification, "computer-readable medium" may be any device that can contain, store, communicate, propagate or transmit a program for use by an instruction execution system, device or apparatus, or in conjunction with such instruction execution systems, devices or apparatuses.

[0234] More specific examples of computer-readable media (a non-exhaustive list) include the following: an electrical connection with one or more wires (electronic device), a portable computer disk case (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disk read-only memory (CDROM). In addition, the computer-readable medium may even be a paper or other suitable medium on which the program is printed, since the program may be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, deciphering or, if necessary, processing in another suitable manner, and then stored in a computer memory.

[0235] It should be understood that the various parts of the present invention can be implemented by hardware, software, firmware or a combination thereof. In the above-mentioned embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or a combination thereof: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0236] The technical features of the above-described embodiments may be arbitrarily combined. To make the description concise, not all possible combinations of the technical features in the above-described embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0237] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

Claims

1. A control system for a rehabilitation robot, wherein the rehabilitation robot is a four-degree-of-freedom serial-parallel upper limb rehabilitation robot, characterized in that: include: A data acquisition module, used to collect the motion data of the rehabilitation robot and the patient's upper limbs in real time, and to construct a corresponding motion control diagram according to the motion data; A data calculation module, used to convert the motion control diagram of the rehabilitation robot into a serial mechanism with the base of the rehabilitation robot as the origin, and to construct a kinematic model of the rehabilitation robot, and to calculate the displacement data of the end effector point E of the rehabilitation robot according to the motion control diagram and the kinematic model; A space calculation module, used for establishing a maximum motion space of the end effector of the rehabilitation robot according to the equalized kinematic model of the rehabilitation robot and the displacement data of the end effector of the rehabilitation robot; A robot control module, used to calculate the effective working space of the rehabilitation robot by using the end effector of the rehabilitation robot and the maximum motion space of the upper limb hemiplegic patient, and to construct a dynamic model of the equalization mechanism of the rehabilitation robot, and to control the rehabilitation robot by using the dynamic model and the effective working space; Wherein, the data acquisition module includes: The data acquisition unit is used to set a distance measurement sensor at the fixed base O point of the rehabilitation robot, and set capture points at the shoulder M, elbow N, and wrist center P of the upper limb hemiplegia patient to collect the distance values ​​of OM, ON, and OP. , , the nine-axis sensor at the wrist center capture point P can measure the spatial direction angle of the forearm ( ), the limb lengths MN and NP of patients with upper limb hemiplegia are known; According to the collection distance OM, ON length , we get two centered at the origin O, The concentric sphere with radius NP can be solved at the same time according to the direction of the linear NP vector measured by the nine-axis sensor at point P and the NP length of the upper limb hemiplegia patient's limb. , with the origin O as the center, The sphere with a radius of 2 is used to solve the coordinates of point M according to the coordinates of point N and the limb length MN of the upper limb hemiplegia patient, so as to calculate the spatial coordinates of the upper limb positioning points M, N, and P of the upper limb hemiplegia patient; A diagram construction unit, used for performing data processing on the motion joint trajectory of the patient's upper limb using a posture settlement algorithm and a corresponding data processing algorithm to construct a motion control diagram of the patient's upper limb; The data calculation module includes: A parameter table construction unit, used for equalizing the series-parallel mechanism of the rehabilitation robot into a corresponding series mechanism based on the motion control diagram of the rehabilitation robot, and performing kinematic analysis on the series mechanism to construct a parameter table of the rehabilitation robot; A data calculation unit, used to construct a kinematic model of the rehabilitation robot with the base of the rehabilitation robot as the origin, and calculate the displacement data of the end effector of the rehabilitation robot according to the parameter table of the rehabilitation robot and the kinematic model; The robot control module is specifically used for: The wrist point P of the upper limb hemiplegia patient is taken as the target point, and the coordinates are marked as , which is equivalent to the spatial position of point E calculated by the rehabilitation robot in series is the actual point, and the coordinates are marked as , according to the spatial direction vector formed by point E and point P at time t , and their distance , determine the movement direction of the rehabilitation robot at time t, then the movement displacement value of the rehabilitation robot is: in, Represents the motor force f and the joint motor angle at time t Functional relationship of Determine whether the motion displacement value of the rehabilitation robot satisfies , , If it exceeds the boundary value, the adjacent endpoint value will be taken and combined with the effective working space of the rehabilitation robot to calculate the actual workload of the rehabilitation robot.

Images