CN114432663A - Six-degree-of-freedom upper limb rehabilitation robot and gravity compensation method thereof - Google Patents
Six-degree-of-freedom upper limb rehabilitation robot and gravity compensation method thereof Download PDFInfo
- Publication number
- CN114432663A CN114432663A CN202210113552.8A CN202210113552A CN114432663A CN 114432663 A CN114432663 A CN 114432663A CN 202210113552 A CN202210113552 A CN 202210113552A CN 114432663 A CN114432663 A CN 114432663A
- Authority
- CN
- China
- Prior art keywords
- servo motor
- cosθ
- direct current
- current servo
- gravity
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 230000005484 gravity Effects 0.000 title claims abstract description 119
- 238000000034 method Methods 0.000 title claims abstract description 61
- 210000001364 upper extremity Anatomy 0.000 title claims abstract description 44
- 230000036544 posture Effects 0.000 claims abstract description 62
- 230000008569 process Effects 0.000 claims abstract description 28
- 230000000694 effects Effects 0.000 claims abstract description 10
- 230000008859 change Effects 0.000 claims abstract description 6
- 210000000323 shoulder joint Anatomy 0.000 claims description 18
- 238000004458 analytical method Methods 0.000 claims description 15
- 210000002310 elbow joint Anatomy 0.000 claims description 13
- 230000003068 static effect Effects 0.000 claims description 11
- 238000010586 diagram Methods 0.000 claims description 8
- 210000000245 forearm Anatomy 0.000 claims description 7
- 238000005452 bending Methods 0.000 claims description 4
- 241000411545 Punargentus Species 0.000 claims description 3
- 239000003638 chemical reducing agent Substances 0.000 claims description 3
- 210000003857 wrist joint Anatomy 0.000 claims description 3
- 238000004590 computer program Methods 0.000 claims description 2
- 238000012937 correction Methods 0.000 claims description 2
- 230000009471 action Effects 0.000 description 4
- 210000003141 lower extremity Anatomy 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 210000003205 muscle Anatomy 0.000 description 2
- 208000012902 Nervous system disease Diseases 0.000 description 1
- 230000032683 aging Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 210000004556 brain Anatomy 0.000 description 1
- 208000026106 cerebrovascular disease Diseases 0.000 description 1
- 239000003814 drug Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000007659 motor function Effects 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 210000000707 wrist Anatomy 0.000 description 1
Images
Classifications
-
- A—HUMAN NECESSITIES
- A63—SPORTS; GAMES; AMUSEMENTS
- A63B—APPARATUS FOR PHYSICAL TRAINING, GYMNASTICS, SWIMMING, CLIMBING, OR FENCING; BALL GAMES; TRAINING EQUIPMENT
- A63B23/00—Exercising apparatus specially adapted for particular parts of the body
- A63B23/035—Exercising apparatus specially adapted for particular parts of the body for limbs, i.e. upper or lower limbs, e.g. simultaneously
- A63B23/12—Exercising apparatus specially adapted for particular parts of the body for limbs, i.e. upper or lower limbs, e.g. simultaneously for upper limbs or related muscles, e.g. chest, upper back or shoulder muscles
-
- A—HUMAN NECESSITIES
- A63—SPORTS; GAMES; AMUSEMENTS
- A63B—APPARATUS FOR PHYSICAL TRAINING, GYMNASTICS, SWIMMING, CLIMBING, OR FENCING; BALL GAMES; TRAINING EQUIPMENT
- A63B21/00—Exercising apparatus for developing or strengthening the muscles or joints of the body by working against a counterforce, with or without measuring devices
- A63B21/00181—Exercising apparatus for developing or strengthening the muscles or joints of the body by working against a counterforce, with or without measuring devices comprising additional means assisting the user to overcome part of the resisting force, i.e. assisted-active exercising
Landscapes
- Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- General Health & Medical Sciences (AREA)
- Physical Education & Sports Medicine (AREA)
- Life Sciences & Earth Sciences (AREA)
- Biophysics (AREA)
- Rehabilitation Tools (AREA)
Abstract
The invention belongs to the field of medical instruments, and relates to a six-degree-of-freedom upper limb rehabilitation robot and a gravity compensation method thereof. The robot comprises a mechanical arm and a control system, wherein the mechanical arm comprises six motors which are connected in series, the control system collects the angle value, the rotating speed value and the current value of each motor in real time in the training process of a patient, and the gravity component born by each motor under different postures of the mechanical arm is analyzed and calculated to calculate the required compensation torque. The movement intention in the training process is captured through the change of the angle, speed, current and other variables of each motor, and based on the calculated compensation torque of each motor in different postures, a motor torque compensation balancing method is adopted to perform torque compensation on each freedom degree motor in real-time postures, so that the gravity compensation effect is realized. The problem of gravity compensation of the upper limb rehabilitation robot in any motion trail is solved, so that the patient is more comfortable and safer in the active training process, and the rehabilitation effect of the patient is improved.
Description
Technical Field
The invention belongs to the field of medical instruments, and relates to a six-degree-of-freedom upper limb rehabilitation robot and a gravity compensation method thereof.
Background
With the aggravation of the aging of the society, the over-fatigue of middle-aged people is more and more serious, a large number of groups of cerebrovascular diseases or nervous system diseases appear, the motor function is damaged, and great inconvenience is brought to daily life. Certain muscle force exists on the upper limbs of some patients in the group, and certain active movement can be completed. According to clinical medicine, active rehabilitation training is the best method for patient rehabilitation efficiency and muscle strength recovery, and can stimulate brain plasticity and improve rehabilitation effect.
In the training process, the patient is in direct contact with the upper limb rehabilitation robot device, and the reaction force of the device can exist when the patient is actively trained. Because the upper limb rehabilitation robot has gravity, if the problem of gravity compensation is not solved, the equipment reaction force and the gravity can act on the upper limb of the patient together, the fatigue of the patient is enhanced, and the patient can be injured.
Patent CN113171271A discloses a gravity compensation method for an upper limb rehabilitation robot, which is to construct a motion trajectory of the multi-joint robot, collect the torque and posture of each joint at different positions and postures in the motion trajectory, and construct a prediction model with the position and posture of the joint as input and the torque of the joint as output to implement gravity compensation. In the method, the motion track of the robot needs to be constructed in advance, different postures in any track cannot be covered, and the friction force of a motor per se is not considered, so that the method cannot be suitable for actual active rehabilitation training.
Patent CN111248917A discloses an active training control method and device for a lower limb walking trainer, which compensates for gravity and friction in any posture of the lower limb walking trainer, and the lower limb walking trainer has two degrees of freedom in common, is simple in posture and easy to analyze, and cannot be suitable for gravity compensation of a robot with more degrees of freedom. Furthermore, this method is implemented without taking into account the influence of the rotational speed on the friction.
In summary, in order to make the patient more comfortable and safer in the active training process and improve the rehabilitation effect of the patient, it is necessary to solve the problem of gravity compensation of the multi-degree-of-freedom upper limb rehabilitation robot in any motion trajectory.
Disclosure of Invention
The invention aims to provide a six-degree-of-freedom upper limb rehabilitation robot and a gravity compensation method thereof, which solve the problem of gravity compensation of the upper limb rehabilitation robot in any motion track, enable a patient to be more comfortable and safer in the active training process, and improve the rehabilitation effect of the patient.
The technical scheme of the invention provides a six-degree-of-freedom upper limb rehabilitation robot, which is characterized in that: the wearable exoskeleton robot comprises a mechanical arm and a control system, wherein the mechanical arm comprises a first direct current servo motor, a second direct current servo motor, a third direct current servo motor, a fourth direct current servo motor and a fourth direct current servo motor which are connected in series; the first direct-current servo motor realizes the internal convergence/abduction of the shoulder joint, the second direct-current servo motor realizes the forward flexion/backward extension of the shoulder joint, the third direct-current servo motor realizes the external rotation/internal rotation of the shoulder joint, the fourth direct-current servo motor realizes the bending/super extension of the elbow joint, the fifth direct-current servo motor realizes the external rotation/internal rotation of the elbow joint, and the sixth direct-current servo motor realizes the dorsiflexion/palmaris flexion of the wrist joint; each direct current servo motor is also provided with a harmonic speed reducer and a driver;
the control system comprises a memory and a processor, the memory having stored therein a computer program which, when executed in the processor, performs the following:
step 1, collecting the angle, the rotating speed and the current value of each direct current servo motor in real time when a patient trains; performing positive kinematic analysis by a D-H parameter method, establishing a six-degree-of-freedom upper limb rehabilitation robot connecting rod coordinate system diagram, and calculating different postures of a mechanical arm in the training process;
and 4, based on the gravity compensation torque required by each direct current servo motor under different postures of the mechanical arm determined in the step 2, precisely controlling each direct current servo motor by adopting a motor torque compensation balancing method under the real-time posture of the mechanical arm, so that the direct current servo motors output torques with equal magnitude and opposite directions are compensated, and the gravity compensation effect is realized.
Further, in the step 2, the second dc servo motor, the third dc servo motor and the fourth dc servo motor are analyzed by a statics theory analysis method, and the gravity components of the robot arm in different postures are calculated to calculate the gravity compensation torque required by the corresponding dc servo motors in different postures of the robot arm.
Further, in step 2, the gravity components of the second dc servo motor, the third dc servo motor and the fourth dc servo motor in different postures of the robot arm are respectively:
T2＇＝[(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx+cosθ2·L1]·G2+cosθ2·Ld·G1
T3＇＝[(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx+cosθ2·L1]·G2
T4＇＝(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx·G2
wherein, T2 ', T3 ' and T4 ' are respectively a second DC servo motor, a third DC servo motor and a fourth DC servo motor, and the gravity components, theta, of the mechanical arm under different postures are2、θ3、θ4Respectively, the angles, L, of the second DC servo motor, the third DC servo motor and the fourth DC servo motorxThe distance between the center of gravity of the forearm and the elbow joint, LdThe distance between the center of gravity of the middle and the large arm of the mechanical arm and the shoulder joint, L1Length of the middle and large arms of the mechanical arm G1、G2Respectively a large arm gravity and a small arm gravity in the mechanical arm.
Further, the gravity compensation torques required by the second dc servo motor, the third dc servo motor and the fourth dc servo motor in different postures of the mechanical arm calculated in the step 2 are respectively:
T2＝[(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx-cosθ2·L1]·G2-cosθ2·Ld·G1
T3＝[(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx-cosθ2·L1]·G2
T4＝(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx·G2
wherein T is2、T3、T4The gravity compensation torque is respectively needed by the second direct current servo motor, the third direct current servo motor and the fourth direct current servo motor.
Further, in order to overcome the friction force and the rotational inertia, the operation is not smooth, and in step 4, each compensation moment T is subjected to resistance compensation coefficient eta i2、T3、T4And correcting, and in the training process, adjusting the gravity compensation torque by adjusting the grade coefficient k, wherein the actual output gravity compensation torque is as follows:
Mi＝k·(ηi·Vi+Ti)
wherein: miGravity compensation torque V for ith DC servo motor actual outputiThe speed of the ith dc servo motor is 2,3, 4.
The invention also provides a gravity compensation method of the six-degree-of-freedom upper limb rehabilitation robot, which is characterized by comprising the following steps of:
step 1, collecting the angle, the rotating speed and the current value of each direct current servo motor in real time when a patient trains; performing positive kinematic analysis by a D-H parameter method, establishing a six-degree-of-freedom upper limb rehabilitation robot connecting rod coordinate system diagram, and calculating different postures of a mechanical arm in the training process;
and 4, based on the gravity compensation torque required by each direct current servo motor under different postures of the mechanical arm determined in the step 2, precisely controlling each direct current servo motor by adopting a motor torque compensation balancing method under the real-time posture of the mechanical arm, so that the direct current servo motors output torques with equal magnitude and opposite directions are compensated, and the gravity compensation effect is realized.
Further, in the step 2, the second dc servo motor, the third dc servo motor and the fourth dc servo motor are analyzed by a statics theory analysis method, and the gravity components of the robot arm in different postures are calculated to calculate the gravity compensation torque required by the corresponding dc servo motor in different postures of the robot arm.
Further, in the step 2, the gravity components of the second dc servo motor, the third dc servo motor and the fourth dc servo motor in different postures of the robot arm are respectively:
T2＇＝[(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx+cosθ2·L1]·G2+cosθ2·Ld·G1
T3＇＝[(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx+cosθ2·L1]·G2
T4＇＝(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx·G2
wherein, T2 ', T3 ' and T4 ' are respectively a second DC servo motor, a third DC servo motor and a fourth DC servo motor, and the gravity components, theta, of the mechanical arm under different postures are2、θ3、θ4Respectively, the angles, L, of the second DC servo motor, the third DC servo motor and the fourth DC servo motorxThe distance between the center of gravity of the forearm and the elbow joint, LdThe distance between the center of gravity of the middle and the large arm of the mechanical arm and the shoulder joint, L1Is the length of the middle and large arms of the mechanical arm G1、G2Which are respectively the gravity of a big arm and the gravity of a small arm in the mechanical arm.
Further, the gravity compensation torques required by the second dc servo motor, the third dc servo motor and the fourth dc servo motor in different postures of the mechanical arm calculated in the step 2 are respectively:
T2＝[(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx-cosθ2·L1]·G2-cosθ2·Ld·G1
T3＝[(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx-cosθ2·L1]·G2
T4＝(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx·G2
wherein T is2、T3、T4The gravity compensation torque is respectively needed by the second direct current servo motor, the third direct current servo motor and the fourth direct current servo motor.
Further, in the step 4, each compensation torque T is compensated by using the resistance compensation coefficient η i2、T3、T4And correcting, and in the training process, adjusting the gravity compensation torque by adjusting the grade coefficient k, wherein the actual output gravity compensation torque is as follows:
Mi＝k·(ηi·Vi+Ti)
wherein: miGravity compensation torque, V, for the actual output of the ith DC servo motoriThe speed of the ith dc servo motor is 2,3, 4.
The beneficial effects of the invention are:
1. aiming at the problems and the defects which are not solved by the prior art, the invention collects the angle value, the rotating speed value and the current value of joint motion in real time in the active training process of a patient, analyzes and calculates different postures of the mechanical arm under different angles and the gravity component of each degree of freedom under different postures, and calculates the required compensation moment. The movement intention in the training process is captured through the change of the angle, the speed, the current and other variables of the motors with the degrees of freedom, and the motor torque compensation balancing method is adopted to perform torque compensation on the motors with the degrees of freedom, so that the gravity compensation of the upper limb rehabilitation robot with the six degrees of freedom in any movement track is realized. The method can make the patient more comfortable and safer in the active training process and improve the rehabilitation effect of the patient.
2. According to the gravity compensation device, the gravity compensation torque is corrected through the resistance compensation coefficient eta i, and in the training process, the gravity compensation torque can be adjusted through adjusting the grade coefficient k, so that the problem of unsmooth operation caused by friction force and rotational inertia is solved.
Drawings
Fig. 1 is a schematic structural diagram of a six-degree-of-freedom upper limb rehabilitation robot joint.
The reference numbers in the figures are: 1-a first direct current servo motor, 2-a second direct current servo motor, 3-a third direct current servo motor, 4-a fourth direct current servo motor, 5-a fifth direct current servo motor and 6-a sixth direct current servo motor;
fig. 2 is a coordinate system diagram of a six-degree-of-freedom upper limb rehabilitation robot connecting rod according to the invention.
Fig. 3 is a diagram of a six-degree-of-freedom upper limb rehabilitation robot gravity compensation statics model.
Fig. 4 is a flow chart of a six-degree-of-freedom upper limb rehabilitation robot gravity compensation method of the invention.
Detailed Description
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, specific embodiments accompanied with figures are described in detail below, and it is apparent that the described embodiments are a part of the embodiments of the present invention, not all of the embodiments. All other embodiments, which can be obtained by a person skilled in the art without making creative efforts based on the embodiments of the present invention, shall fall within the protection scope of the present invention.
In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present invention, but the present invention may be practiced in other ways than those specifically described and will be readily apparent to those of ordinary skill in the art without departing from the spirit of the present invention, and therefore the present invention is not limited to the specific embodiments disclosed below. Furthermore, the terms first, second … … sixth are used for descriptive purposes only and are not to be construed as indicating or implying relative importance.
As can be seen from fig. 1, the six-degree-of-freedom upper limb rehabilitation robot in the embodiment is in a wearable exoskeleton form and comprises a mechanical arm, wherein the mechanical arm realizes 6 degrees of freedom of motion by adopting six series-connected direct current servo motors, and can realize automatic switching of a left arm and a right arm according to use requirements. The first direct-current servo motor realizes the internal convergence/abduction of the shoulder joint, the second direct-current servo motor realizes the forward flexion/backward extension of the shoulder joint, the third direct-current servo motor realizes the external rotation/internal rotation of the shoulder joint, the fourth direct-current servo motor realizes the bending/super extension of the elbow joint, the fifth direct-current servo motor realizes the external rotation/internal rotation of the elbow joint, and the sixth direct-current servo motor realizes the dorsiflexion/palmaris flexion of the wrist joint.
Each direct current servo motor is also provided with a harmonic speed reducer and a driver, and each six-degree-of-freedom upper limb rehabilitation robot is provided with a control system to realize gravity compensation and accurate control of each motor.
In this embodiment, the six-degree-of-freedom upper limb rehabilitation robot can realize the training modes such as the passive mode, the active and passive mode, the active mode, the teaching mode and the like, and the gravity compensation is required in the active training process of the active and passive mode, the active mode and the teaching mode.
As shown in fig. 2, in the present embodiment, the six-degree-of-freedom upper limb rehabilitation robot shown in fig. 1 is subjected to positive kinematic analysis by using a D-H parameter method, and a six-degree-of-freedom upper limb rehabilitation robot link coordinate system diagram is established, where the six-degree-of-freedom upper limb rehabilitation robot link parameters are shown in the following table.
| i | ai-1 | αi-1 | di | θi |
| 1 | 0 | 0 | 0 | θ1(0) |
| 2 | 0 | 90° | 0 | θ2(90) |
| 3 | 0 | 90° | L1 | θ3(90) |
| 4 | 0 | 90° | 0 | θ4(0) |
| 5 | 0 | -90° | L2 | θ5(-90) |
| 6 | L3 | -90° | 0 | θ6(-90) |
In the above table: i is an ith connecting rod (composed of a first direct current servo motor to a sixth direct current servo motor) of the mechanical arm; a isi-1Is Zi-1Axis to ZiDistance between axes, αi-1Is the Z thi-1Axis to ZiTorsion angle between shafts, diIs the Xi-1Axis to XiDistance between axes, θiL1, L2 and L3 respectively represent the length of the big arm, the length of the small arm and the length of the wrist of the mechanical arm for the rotation angle of each joint and the initial angle corresponding to the coordinate system.
According to the D-H parameter method, the following can be calculated:
the projection of the gravity of the large arm of the mechanical arm in the gravity direction of the base scale is cos theta2。
The projection of the mechanical arm forearm gravity in the direction of the base standard gravity is-sin theta2·sinθ4·cosθ3-cosθ2·cosθ3。
As shown in fig. 3, the first dc servo motor, the fifth dc servo motor, and the sixth dc servo motor are less influenced by gravity during rotation, so in this embodiment, a statics theory analysis method is mainly used to analyze the gravity components of the second dc servo motor, the third dc servo motor, and the fourth dc servo motor of the six-degree-of-freedom upper limb rehabilitation robot in different postures, and calculate the required compensation torque.
Taking Lx as the distance between the gravity center of the middle and lower arm of the mechanical arm and the elbow joint, Ld as the distance between the gravity center of the middle and lower arm of the mechanical arm and the shoulder joint, and G1 and G2 are respectively the gravity of the middle and lower arm of the mechanical arm, and the analysis shows that:
when the forearm rotates around the elbow joint, the projection of the force arm in the gravity direction is as follows:
(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx
when the big arm rotates around the shoulder joint, the projection of the force arm in the gravity direction is as follows: cos θ2·Ld
When the forearm rotates around the shoulder joint, the projection of the force arm in the gravity direction is as follows:
(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx+cosθ2·L1
the second direct current servo motor realizes the action of forward flexion/backward extension of the shoulder joint, and the action of overcoming the gravity at any posture is as follows:
[(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx+cosθ2·L1]·G2+cosθ2·Ld·G1
the third direct current servo motor realizes the action of internal rotation/external rotation of the shoulder joint, and the action of overcoming the gravity at any posture is as follows:
[(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx+cosθ2·L1]·G2
the motion realized by the fourth direct current servo motor is used as elbow joint bending/super-stretching, and the motion needs to overcome gravity at any posture:
(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx·G2
therefore, when gravity compensation is performed, the torques required to be compensated by the second direct current servo motor, the third direct current servo motor and the fourth direct current servo motor are respectively as follows:
T2＝[(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx-cosθ2·L1]·G2-cosθ2·Ld·G1
T3＝[(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx-cosθ2·L1]·G2
T4＝(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx·G2
wherein, T2, T3, and T4 are gravity compensation torques required by the second dc servo motor, the third dc servo motor, and the fourth dc servo motor, respectively.
In order to overcome the defects of friction force and rotational inertia, the operation is not smooth and flexible, the correction is carried out through a resistance compensation coefficient eta i, the gravity compensation grade can be adjusted through adjusting a grade coefficient k in the training process, and the actual output torque is as follows:
the actual output torque is:
Mi＝k·(ηi·Vi+Ti)
wherein: miGravity compensation torque, V, for the actual output of the ith DC servo motoriThe speed of the ith dc servo motor is 2,3, 4.
In the present application example, the mechanical arm parameter is G1-35N, G2-25N, L1-320 mm, L2-250 mm, Ld-200 mm, Lx-150 mm, and the chosen drag compensation coefficient is η2＝0.003，
η4And k is 0.003, is selected to be 0.5-1.3 and is adjustable, and the supporting degree is represented.
With reference to fig. 1 to 4, the gravity compensation method for the six-degree-of-freedom upper limb rehabilitation robot in the embodiment includes the following main operation processes:
1) in the training process, the six-degree-of-freedom upper limb rehabilitation robot needs to acquire parameters such as the position, the speed and the current of each direct current servo motor in real time;
2) establishing a D-H coordinate system through the real-time positions of all motors, and analyzing different postures of the six-degree-of-freedom upper limb rehabilitation robot mechanical arm in the training process;
3) analyzing by using a statics theory, analyzing each direct current servo motor, calculating gravity components of the mechanical arm in different postures, and calculating gravity compensation torque required by each direct current servo motor in different postures of the mechanical arm;
4) analyzing the variation trend of parameters such as motor position, speed and current, capturing the movement intention in the training process, and obtaining the real-time posture of the mechanical arm;
5) a motor torque compensation balancing method is adopted to accurately control each motor and output a torque which is equal to the gravity component in magnitude and opposite in direction; the gravity compensation is realized, so that the patient is more comfortable and safer in the active training process.
The above-mentioned embodiments are intended to illustrate the objects, technical solutions and advantages of the present invention in further detail, and it should be understood that the above-mentioned embodiments are merely exemplary embodiments of the present invention, and are not intended to limit the scope of the present invention, and any modifications, equivalent substitutions, improvements and the like made within the spirit and principle of the present invention should be included in the scope of the present invention.
Claims (10)
1. A six-degree-of-freedom upper limb rehabilitation robot is characterized in that: the exoskeleton robot is in a wearable exoskeleton form and comprises a mechanical arm and a control system, wherein the mechanical arm comprises first to sixth direct current servo motors which are connected in series; the first direct-current servo motor realizes the internal convergence/abduction of the shoulder joint, the second direct-current servo motor realizes the forward flexion/backward extension of the shoulder joint, the third direct-current servo motor realizes the external rotation/internal rotation of the shoulder joint, the fourth direct-current servo motor realizes the bending/super extension of the elbow joint, the fifth direct-current servo motor realizes the external rotation/internal rotation of the elbow joint, and the sixth direct-current servo motor realizes the dorsiflexion/palmaris flexion of the wrist joint; each direct current servo motor is also provided with a harmonic speed reducer and a driver;
the control system comprises a memory and a processor, the memory having stored therein a computer program which, when executed in the processor, performs the following:
step 1, collecting the angle, the rotating speed and the current value of each direct current servo motor in real time when a patient trains; performing positive kinematic analysis by a D-H parameter method, establishing a six-degree-of-freedom upper limb rehabilitation robot connecting rod coordinate system diagram, and calculating different postures of a mechanical arm in the training process;
step 2, analyzing the gravity components of the direct current servo motors under different postures of the mechanical arm through a statics theory analysis method, and calculating the gravity compensation torque required by each direct current servo motor under different postures of the mechanical arm;
step 3, capturing movement intentions in the training process through the angle, speed and current change trends of each direct current servo motor to obtain the real-time posture of the mechanical arm;
and 4, based on the gravity compensation torque required by each direct current servo motor under different postures of the mechanical arm determined in the step 2, precisely controlling each direct current servo motor by adopting a motor torque compensation balancing method under the real-time posture of the mechanical arm, so that the direct current servo motors output torques with equal magnitude and opposite directions are compensated, and the gravity compensation effect is realized.
2. The six degree-of-freedom upper limb rehabilitation robot of claim 1, wherein: and 2, analyzing the gravity components of the second direct current servo motor, the third direct current servo motor and the fourth direct current servo motor under different postures of the mechanical arm by a statics theory analysis method, and calculating the gravity compensation torque required by the corresponding direct current servo motor under different postures of the mechanical arm.
3. The six degree-of-freedom upper limb rehabilitation robot of claim 2, wherein: in the step 2, the gravity components of the second direct current servo motor, the third direct current servo motor and the fourth direct current servo motor under different postures of the mechanical arm are respectively as follows:
T2＇＝[(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx+cosθ2·L1]·G2+cosθ2·Ld·G1
T3＇＝[(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx+cosθ2·L1]·G2
T4＇＝(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx·G2
wherein, T2 ', T3 ' and T4 ' are respectively a second DC servo motor, a third DC servo motor and a fourth DC servo motor, and the gravity components, theta, of the mechanical arm under different postures are2、θ3、θ4Respectively, the angles, L, of the second DC servo motor, the third DC servo motor and the fourth DC servo motorxThe distance between the center of gravity of the forearm and the elbow joint, LdThe distance between the center of gravity of the middle and the large arm of the mechanical arm and the shoulder joint, L1Is the length of the middle and large arms of the mechanical arm G1、G2Respectively a large arm gravity and a small arm gravity in the mechanical arm.
4. The six degree-of-freedom upper limb rehabilitation robot of claim 3, wherein: the gravity compensation torques required by the second direct current servo motor, the third direct current servo motor and the fourth direct current servo motor under different postures of the mechanical arm calculated in the step 2 are respectively as follows:
T2＝[(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx-cosθ2·L1]·G2-cosθ2·Ld·G1
T3＝[(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx-cosθ2·L1]·G2
T4＝(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx·G2
wherein T is2、T3、T4The gravity compensation torque is respectively needed by the second direct current servo motor, the third direct current servo motor and the fourth direct current servo motor.
5. The six degree-of-freedom upper limb rehabilitation robot of claim 4, wherein: step 4, utilizing the resistance compensation coefficient eta i to compensate each compensation moment T2、T3、T4And correcting, and in the training process, adjusting the gravity compensation torque by adjusting the grade coefficient k, wherein the actual output gravity compensation torque is as follows:
Mi＝k·(ηi·Vi+Ti)
wherein: miGravity compensation torque, V, for the actual output of the ith DC servo motoriThe speed of the ith dc servo motor is 2,3, 4.
6. A gravity compensation method of a six-degree-of-freedom upper limb rehabilitation robot is characterized by comprising the following steps:
step 1, collecting the angle, the rotating speed and the current value of each direct current servo motor in real time when a patient trains; performing positive kinematic analysis by a D-H parameter method, establishing a six-degree-of-freedom upper limb rehabilitation robot connecting rod coordinate system diagram, and calculating different postures of a mechanical arm in the training process;
step 2, analyzing the gravity components of the direct current servo motors under different postures of the mechanical arm through a statics theory analysis method, and calculating the gravity compensation torque required by each direct current servo motor under different postures of the mechanical arm;
step 3, capturing movement intentions in the training process through the angle, speed and current change trends of each direct current servo motor to obtain the real-time posture of the mechanical arm;
and 4, based on the gravity compensation torque required by each direct current servo motor under different postures of the mechanical arm determined in the step 2, precisely controlling each direct current servo motor by adopting a motor torque compensation balancing method under the real-time posture of the mechanical arm, so that the direct current servo motors output torques with equal magnitude and opposite directions are compensated, and the gravity compensation effect is realized.
7. The gravity compensation method for a six-degree-of-freedom upper limb rehabilitation robot according to claim 6, characterized in that: and 2, analyzing the gravity components of the second direct current servo motor, the third direct current servo motor and the fourth direct current servo motor under different postures of the mechanical arm by a statics theory analysis method, and calculating the gravity compensation torque required by the corresponding direct current servo motor under different postures of the mechanical arm.
8. The gravity compensation method for a six-degree-of-freedom upper limb rehabilitation robot according to claim 7, characterized in that: in the step 2, the gravity components of the second direct current servo motor, the third direct current servo motor and the fourth direct current servo motor under different postures of the mechanical arm are respectively as follows:
T2＇＝[(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx+cosθ2·L1]·G2+cosθ2·Ld·G1
T3＇＝[(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx+cosθ2·L1]·G2
T4＇＝(-sinθ2·sinθ4·cosθ3-cosθ2·cosθ3)·Lx·G2
wherein, T2 ', T3 ' and T4 ' are respectively the second direct currentThe servo motor, the third direct current servo motor and the fourth direct current servo motor are used for controlling the gravity component theta of the mechanical arm under different postures2、θ3、θ4Respectively, the angles, L, of the second DC servo motor, the third DC servo motor and the fourth DC servo motorxThe distance between the center of gravity of the forearm and the elbow joint, LdThe distance between the center of gravity of the middle and the large arm of the mechanical arm and the shoulder joint, L1Is the length of the middle and large arms of the mechanical arm G1、G2Respectively a large arm gravity and a small arm gravity in the mechanical arm.
9. The gravity compensation method for a six-degree-of-freedom upper limb rehabilitation robot according to claim 8, characterized in that: the gravity compensation torques required by the second direct current servo motor, the third direct current servo motor and the fourth direct current servo motor under different postures of the mechanical arm calculated in the step 2 are respectively as follows:
T2＝[(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx-cosθ2·L1]·G2-cosθ2·Ld·G1
T3＝[(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx-cosθ2·L1]·G2
T4＝(sinθ2·sinθ4·cosθ3+cosθ2·cosθ3)·Lx·G2
wherein T is2、T3、T4The gravity compensation torque is respectively needed by the second direct current servo motor, the third direct current servo motor and the fourth direct current servo motor.
10. The gravity compensation method for a six-degree-of-freedom upper limb rehabilitation robot according to claim 9, characterized in that: step 4, utilizing the resistance compensation coefficient eta i to compensate each compensation moment T2、T3、T4Making corrections, and during training, by adjusting, etcThe stage coefficient k adjusts the gravity compensation moment, and the actual output gravity compensation moment is as follows:
Mi＝k·(ηi·Vi+Ti)
wherein: miGravity compensation torque, V, for the actual output of the ith DC servo motoriThe speed of the ith dc servo motor is 2,3, 4.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210113552.8A CN114432663A (en) | 2022-01-30 | 2022-01-30 | Six-degree-of-freedom upper limb rehabilitation robot and gravity compensation method thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210113552.8A CN114432663A (en) | 2022-01-30 | 2022-01-30 | Six-degree-of-freedom upper limb rehabilitation robot and gravity compensation method thereof |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114432663A true CN114432663A (en) | 2022-05-06 |
Family
ID=81371034
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210113552.8A Pending CN114432663A (en) | 2022-01-30 | 2022-01-30 | Six-degree-of-freedom upper limb rehabilitation robot and gravity compensation method thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114432663A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116492201A (en) * | 2023-06-21 | 2023-07-28 | 山东泽普医疗科技有限公司 | Gravity compensation method and control system of three-dimensional upper limb rehabilitation training instrument |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2007135888A (en) * | 2005-11-18 | 2007-06-07 | Bridgestone Sports Co Ltd | Iron golf club set |
| CN108748147A (en) * | 2018-06-01 | 2018-11-06 | 清华大学深圳研究生院 | A kind of control system and method for ectoskeleton mechanical arm |
| CN111248917A (en) * | 2020-01-17 | 2020-06-09 | 哈工大机器人（合肥）国际创新研究院 | Active training control method and device for lower limb walking trainer |
| CN111345970A (en) * | 2020-03-12 | 2020-06-30 | 安徽工业大学 | Seven-degree-of-freedom upper limb exoskeleton rehabilitation robot |
| CN113171271A (en) * | 2021-04-30 | 2021-07-27 | 华中科技大学 | Gravity compensation method for upper limb rehabilitation robot |
| CN113352294A (en) * | 2020-03-06 | 2021-09-07 | 现代自动车株式会社 | Gravity compensator applied to wearable muscle strength auxiliary device |
| CN113413300A (en) * | 2021-06-29 | 2021-09-21 | 长春工业大学 | Real-time gravity compensation four-degree-of-freedom shoulder-elbow joint rehabilitation training device |
| CN113730178A (en) * | 2021-07-27 | 2021-12-03 | 深圳华鹊景医疗科技有限公司 | Upper limb constant-speed training rehabilitation robot system and robot control method |
-
2022
- 2022-01-30 CN CN202210113552.8A patent/CN114432663A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2007135888A (en) * | 2005-11-18 | 2007-06-07 | Bridgestone Sports Co Ltd | Iron golf club set |
| CN108748147A (en) * | 2018-06-01 | 2018-11-06 | 清华大学深圳研究生院 | A kind of control system and method for ectoskeleton mechanical arm |
| CN111248917A (en) * | 2020-01-17 | 2020-06-09 | 哈工大机器人（合肥）国际创新研究院 | Active training control method and device for lower limb walking trainer |
| CN113352294A (en) * | 2020-03-06 | 2021-09-07 | 现代自动车株式会社 | Gravity compensator applied to wearable muscle strength auxiliary device |
| CN111345970A (en) * | 2020-03-12 | 2020-06-30 | 安徽工业大学 | Seven-degree-of-freedom upper limb exoskeleton rehabilitation robot |
| CN113171271A (en) * | 2021-04-30 | 2021-07-27 | 华中科技大学 | Gravity compensation method for upper limb rehabilitation robot |
| CN113413300A (en) * | 2021-06-29 | 2021-09-21 | 长春工业大学 | Real-time gravity compensation four-degree-of-freedom shoulder-elbow joint rehabilitation training device |
| CN113730178A (en) * | 2021-07-27 | 2021-12-03 | 深圳华鹊景医疗科技有限公司 | Upper limb constant-speed training rehabilitation robot system and robot control method |
Non-Patent Citations (1)
| Title |
|---|
| 冷炳营: "《多功能上肢康复机器人主被动控制算法研究》", 《信息科技辑》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116492201A (en) * | 2023-06-21 | 2023-07-28 | 山东泽普医疗科技有限公司 | Gravity compensation method and control system of three-dimensional upper limb rehabilitation training instrument |
| CN116492201B (en) * | 2023-06-21 | 2023-10-03 | 山东泽普医疗科技有限公司 | Gravity compensation method and control system of three-dimensional upper limb rehabilitation training instrument |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Zeiaee et al. | Design and kinematic analysis of a novel upper limb exoskeleton for rehabilitation of stroke patients | |
| Rahman et al. | EMG based control of a robotic exoskeleton for shoulder and elbow motion assist | |
| Wang | Three-dimensional kinematic analysis of influence of hand orientation and joint limits on the control of arm postures and movements | |
| CN109199783B (en) | Control method for controlling stiffness of ankle joint rehabilitation equipment by using sEMG | |
| CN113633521A (en) | Control system and control method for upper limb exoskeleton rehabilitation robot | |
| He et al. | Mechatronic design of a synergetic upper limb exoskeletal robot and wrench-based assistive control | |
| Wang et al. | Active training research of a lower limb rehabilitation robot based on constrained trajectory | |
| CN114432663A (en) | Six-degree-of-freedom upper limb rehabilitation robot and gravity compensation method thereof | |
| Culmer et al. | An admittance control scheme for a robotic upper-limb stroke rehabilitation system | |
| Rahman et al. | Dynamic modeling and evaluation of a robotic exoskeleton for upper-limb rehabilitation | |
| CN109887570B (en) | Robot-assisted rehabilitation training method based on RGB-D camera and IMU sensor | |
| CN114711760B (en) | Joint axis calculation method | |
| Tageldeen et al. | Motion control for a multiple input rehabilitation wearable exoskeleton using fuzzy logic and PID | |
| Guo et al. | Serial–parallel mechanism and controller design of a robotic brace for dynamic trunk support | |
| CN113197754B (en) | Upper limb exoskeleton rehabilitation robot system and method | |
| Xu et al. | Design and postural synergy synthesis of a prosthetic hand for a manipulation task | |
| Jung et al. | Learning classifier to evaluate movement quality in unassisted pick-and-place exercises for post-stroke patients: A preliminary study | |
| Wang et al. | A cable-driven parallel hip exoskeleton for high-performance walking assistance | |
| Guo et al. | Admittance control of wearable robotic brace for dynamic trunk support | |
| Zhang et al. | Stress analysis of key components of finger rehabilitation exoskeleton robot based on finite element method | |
| Fekrache et al. | Kinematics and design of a 5 DoF exoskeleton for the rehabilitation of the upper limb | |
| Madhushan et al. | Hand rehabilitation using robot-assisted physiotherapy | |
| Tao et al. | Design of a Desktop Upper Limb Rehabilitation Robot | |
| Cai et al. | Dual-Arm Upper Limb Rehabilitation Robot: Mechanism Design and Preliminary Experiments | |
| Kwon et al. | Real-time upper limb motion prediction from noninvasive biosignals for physical human-machine interactions |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20220506 |