CN116492201A - Gravity compensation method and control system of three-dimensional upper limb rehabilitation training instrument - Google Patents
Gravity compensation method and control system of three-dimensional upper limb rehabilitation training instrument Download PDFInfo
- Publication number
- CN116492201A CN116492201A CN202310735480.5A CN202310735480A CN116492201A CN 116492201 A CN116492201 A CN 116492201A CN 202310735480 A CN202310735480 A CN 202310735480A CN 116492201 A CN116492201 A CN 116492201A
- Authority
- CN
- China
- Prior art keywords
- joint
- gravity
- angle
- moment
- real
- 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.)
- Granted
Links
- 230000005484 gravity Effects 0.000 title claims abstract description 124
- 238000000034 method Methods 0.000 title claims abstract description 37
- 210000001364 upper extremity Anatomy 0.000 title claims abstract description 28
- 238000013500 data storage Methods 0.000 claims abstract description 25
- 238000004364 calculation method Methods 0.000 claims abstract description 23
- 239000000284 extract Substances 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000033001 locomotion Effects 0.000 abstract description 15
- 230000000694 effects Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000005540 biological transmission Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 210000003414 extremity Anatomy 0.000 description 2
- 210000000245 forearm Anatomy 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 208000036487 Arthropathies Diseases 0.000 description 1
- 208000012902 Nervous system disease Diseases 0.000 description 1
- 208000025966 Neurological disease Diseases 0.000 description 1
- 238000005299 abrasion Methods 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000032683 aging Effects 0.000 description 1
- 208000029028 brain injury Diseases 0.000 description 1
- 238000013075 data extraction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000004064 dysfunction Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 210000003205 muscle Anatomy 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Classifications
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
- A61H1/00—Apparatus for passive exercising; Vibrating apparatus; Chiropractic devices, e.g. body impacting devices, external devices for briefly extending or aligning unbroken bones
- A61H1/02—Stretching or bending or torsioning apparatus for exercising
- A61H1/0214—Stretching or bending or torsioning apparatus for exercising by rotating cycling movement
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
- A61H1/00—Apparatus for passive exercising; Vibrating apparatus; Chiropractic devices, e.g. body impacting devices, external devices for briefly extending or aligning unbroken bones
- A61H1/02—Stretching or bending or torsioning apparatus for exercising
- A61H1/0274—Stretching or bending or torsioning apparatus for exercising for the upper limbs
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
- A61H2201/00—Characteristics of apparatus not provided for in the preceding codes
- A61H2201/12—Driving means
- A61H2201/1207—Driving means with electric or magnetic drive
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
- A61H2201/00—Characteristics of apparatus not provided for in the preceding codes
- A61H2201/50—Control means thereof
- A61H2201/5058—Sensors or detectors
- A61H2201/5069—Angle sensors
Landscapes
- Health & Medical Sciences (AREA)
- Epidemiology (AREA)
- Pain & Pain Management (AREA)
- Physical Education & Sports Medicine (AREA)
- Rehabilitation Therapy (AREA)
- Life Sciences & Earth Sciences (AREA)
- Animal Behavior & Ethology (AREA)
- General Health & Medical Sciences (AREA)
- Public Health (AREA)
- Veterinary Medicine (AREA)
- Rehabilitation Tools (AREA)
Abstract
The invention discloses a gravity compensation method and a control system of a three-dimensional upper limb rehabilitation training device, which belong to the technical field of gravity compensation and comprise the following steps: step 1, performing position scanning on the three joint full-posture spaces, recording joint angles and gravity moments of each joint under different angles, performing gravity calibration, and finally storing the gravity calibration into a corresponding data storage unit; step 2, obtaining a real-time angle through measuring the position of the mechanical arm in real time, and then extracting the heavy moment from the corresponding data storage unit; and 3, respectively measuring real-time resultant moment of the three joints of the mechanical arm, and carrying out gravity compensation operation according to the gravity moment extracted in the step 2. The invention has simple modeling calculation aiming at the movement of a plurality of joints, is suitable for complex movement structures and has small calculation amount.
Description
Technical Field
The invention relates to the technical field of gravity compensation, in particular to a gravity compensation method and a control system of a three-dimensional upper limb rehabilitation training instrument.
Background
For some patients with upper limb movement joint dysfunction caused by brain injury or neurological diseases, rehabilitation training is an important link for eliminating or relieving the upper limb dysfunction, and based on rehabilitation medical theory, the rehabilitation training is used for assisting the gradual recovery of the lost functions of the affected limbs through repeated movement training of the affected limbs. The three-dimensional upper limb rehabilitation training device simulates the motion law of the upper limb of a human body in real time, can realize the motion of the upper limb in multiple dimensions, and can perform rehabilitation training under the condition of complete zero muscle force by combining scenario interaction, training feedback information and a powerful evaluation system, so that the progress of the rehabilitation training of the patient is advanced.
The three-dimensional upper limb rehabilitation training device comprises a mechanical arm and a driving assembly, wherein the mechanical arm comprises a front arm and a rear arm, the front arm and the rear arm are movably connected, the mechanical arm is arranged on the driving assembly through the rear end of the rear arm, the driving assembly can drive the rear arm to move up and down, left and right and the front arm to rotate relative to the rear arm, the three joints are connected in series to move, and each joint is provided with a corresponding torque sensor and an angle sensor. The arm of the rehabilitation person is fixed at the tail end of the forearm for rehabilitation training, and because of the gravity effect (only the static or low-speed movement condition is considered) under different angle positions, the gravity of the three joints of the mechanical arm can influence the data value acquired by the torque sensor and can not completely reflect the condition of the acting force of the tail end of the forearm, so that the movement control of the mechanical arm can not be carried out through the acting force, and therefore, in order to accurately reflect the acting force of the tail end, gravity compensation is needed.
The Chinese patent with application number 202210064467.7 discloses a gravity compensation method of an upper limb rehabilitation robot, which comprises the steps of firstly obtaining a target position, and calculating a target joint angle of each joint through a target position parameter; calculating the arm support moment required by each joint according to the target joint angle, the actual joint angle corresponding to the target joint angle, the target joint angular velocity and the actual joint angular velocity; finally, calculating to obtain the output moment of each joint according to the target motion parameters and the actual motion parameters of each joint, and calculating to obtain the driving quantity of each joint according to the output moment of each joint; and further controlling the joint to be trained to apply an auxiliary torque according to the driving quantity so as to perform rehabilitation training.
When the method calculates the target joint angle, a DH coordinate system is required to be established for the upper limb rehabilitation robot, the terminal homogeneous transformation matrix and the linear velocity jacobian matrix are determined, a calculation convergence threshold value and an iteration frequency threshold value are set, and the following parameters are calculated in a circulating way in sequence: position error, joint angle, cycle number plus one; the arm support moment required by each joint is calculated by a target joint angle, an actual joint angle corresponding to the target joint angle, a target joint angular velocity and an actual joint angular velocity based on an iterative learning method; in addition, the gravity torque error, the driving amount of each joint, and the like are calculated by a series of formulas. Therefore, modeling calculation of the method is complex, engineering quantity can be greatly increased, particularly, movement calculation quantity of a plurality of joints is large, errors are generated in each data extraction process and each data calculation process, new errors are introduced after gravity compensation, and the errors are accumulated together to cause inaccuracy of final gravity compensation, so that ideal gravity compensation effect is difficult to realize, and movement accuracy of rehabilitation training is further affected.
Disclosure of Invention
Aiming at the defects in the background technology, the invention provides a gravity compensation method and a control system of a three-dimensional upper limb rehabilitation training device, which are simple in modeling calculation aiming at the movement of a plurality of joints, suitable for complex movement structures and small in calculation amount.
In order to solve the technical problems, the invention adopts the following technical scheme:
a gravity compensation method of a three-dimensional upper limb rehabilitation training device comprises the following steps:
step 1, performing position scanning on the three joint full-posture spaces, recording joint angles and gravity moments of each joint under different angles, performing gravity calibration, and finally storing the gravity calibration into a corresponding data storage unit;
step 2, obtaining a real-time angle through measuring the position of the mechanical arm in real time, and then extracting the heavy moment from the corresponding data storage unit;
and 3, respectively measuring real-time resultant moment of the three joints of the mechanical arm, and carrying out gravity compensation operation according to the gravity moment extracted in the step 2.
Further, in step 3, the operation formula of the gravity compensation operation is as follows:
, wherein ,/>、/>、/>The arm of the rehabilitee applies acting force to the tail end of the front arm of the mechanical arm for external moment respectively applied to the three joints, wherein the external moment is the force transmitted to each joint by the acting force, and the joint is a joint>、/>、/>For the weight moment extracted by the three joints from the data storage unit corresponding to the position of the joints, +.>、/>、/>The real-time resultant moment is obtained by respectively measuring three joints, and the resultant moment is the moment of the joints after the joints bear external moment and heavy moment.
Further, in the gravity calibration in the step 1, calibration sampling is performed at intervals of 5 degrees.
Further, the three joints are joint one, joint two and joint three respectively, and the mechanical arm comprises a front arm and a rear arm;
the first joint is driven by a first driving component to realize the up-and-down swing of the mechanical arm, and the second joint is driven by a second driving component to realize the left-and-right rotation of the mechanical arm; and the joint III is driven by a third driving assembly to realize the rotation of the rear end of the front arm relative to the front end of the rear arm.
Further, in step 1, the method of the position scanning is as follows,
returning each joint to an initial angle position;
then, gradually changing the angle positions of the joint from the initial angle at intervals of 5 degrees, and scanning the corresponding gravity moment at each angle position; each angular position of the first joint corresponds to a plurality of gravity moments of different angular positions of the second joint and the third joint;
when each angle position of the joint I is scanned, the joint II and the joint III are returned to the initial angle positions, then the angle positions of the joint III are gradually changed every 5 degrees until the angle position of the joint III exceeds the end angle position, the joint II is gradually changed every 5 degrees, and in each position of the joint II, the joint III moves from the initial angle position to the end angle position until the angle position of the joint II exceeds the end angle position, the angle position of the joint I is gradually changed every 5 degrees until the angle position of the joint I reaches the end angle position.
Further, in step 2, after extracting the heavy moment, the method for calculating the heavy moment corresponding to the real-time angle according to the extracted heavy moment is as follows:
assume that the real-time angle x is at the nominal angle x 0 and x1 The calculation formula is as follows:
wherein ：、/>、/>respectively is an angle x, x 0 and x1 Lower gravitational moment.
Further, each joint is provided with a torque sensor and an angle sensor, and when the position scanning is performed in the step 1, the torque corresponding to each joint at different angles is recorded through the angle sensor and the torque sensor respectively; in the step 2, measuring a real-time angle through an angle sensor; in the step 3, the torque sensor measures the real-time resultant moment of the three joints of the mechanical arm.
Also comprises a gravity compensation control system of the three-dimensional upper limb rehabilitation training device,
the device comprises a controller, an angle sensor, a torque sensor, a position scanning module, a data storage unit and a real-time measurement module.
The controller comprises an interpolation calculation module and a compensation calculation module, wherein the interpolation calculation module extracts the corresponding gravity moment in the data storage unit and calculates the gravity moment of the real-time angle; the compensation operation module obtains a real-time angle and calculates external moments respectively born by the three joints according to the gravity moment of the real-time angle and the real-time resultant moment of each joint, so as to perform gravity compensation;
the position scanning module is used for carrying out position scanning on the full-posture space of each joint, recording joint angles and gravity moments of each joint under different angles and carrying out gravity calibration;
the data storage unit is used for storing the gravity moment corresponding to the joint angle during gravity calibration so as to be extracted by the controller;
the real-time measuring module is used for measuring the position of the mechanical arm in real time to obtain a real-time angle for the controller to extract;
the angle sensor and the torque sensor record the angle and the torque of each joint at each position respectively.
After the technical scheme is adopted, compared with the prior art, the invention has the following advantages:
1. the gravity calibration can be one-time calibration when leaving the factory, and the calibration is stored in the corresponding data storage unit, calibration and calculation are not needed each time, and the calibrated gravity moment can be extracted at any time.
2. The gravity compensation method is characterized in that the gravity compensation method is used for realizing the compensation of the mechanical arm on the gravity under the complex mechanical environment, the gravity calibration mode is adopted, the gravity of each mechanism under the influence of the gravity is scanned and calibrated, then the gravity compensation under the real-time angle is matched from stored data, the real-time resultant moment of the three joints, which are respectively measured, is subtracted by the gravity moment extracted from the data storage unit corresponding to the positions of the three joints, so that the external moment respectively born by the three joints is obtained.
The invention will now be described in detail with reference to the drawings and examples.
Drawings
FIG. 1 is a logic diagram of the gravity compensation of the present invention;
FIG. 2 is a block diagram of the gravity compensation control system of the present invention;
FIG. 3 is a schematic diagram of the structure of a three-dimensional upper limb rehabilitation training device;
fig. 4 is a logic flow diagram of a position scan.
In the figures, 1-first drive assembly, 2-second drive assembly, 3-third drive assembly, 4-rear arm, 5-front arm.
Detailed Description
For a clearer understanding of technical features, objects, and effects of the present invention, a specific embodiment of the present invention will be described with reference to the accompanying drawings.
As shown in fig. 1, the invention provides a gravity compensation method of a three-dimensional upper limb rehabilitation training device, comprising the following steps:
step 1, scanning the three joints in full-attitude space positions, recording joint angles and gravity moments of the joints under different angles, calibrating gravity, and finally storing the gravity calibration in a corresponding data storage unit;
step 2, obtaining a real-time angle through measuring the position of the mechanical arm in real time, and then extracting the heavy moment from the corresponding data storage unit;
and 3, respectively measuring the real-time resultant moment of each joint of the mechanical arm, and carrying out gravity compensation operation according to the gravity moment extracted in the step 2. The operation formula of the gravity compensation operation is as follows:
wherein ,、/>、/>the external moment is applied to the three joints respectively, the arm of the rehabilitation person applies acting force to the tail end of the front arm of the mechanical arm, and the external moment is force transmitted to each joint by the acting force; />、/>、/>The gravity moment is extracted from the data storage unit corresponding to the positions of the three joints; />、/>、/>The real-time resultant moment is obtained by respectively measuring three joints, and the resultant moment is the moment of the joints after the joints bear external moment and heavy moment.
As shown in fig. 3, the three joints are joint one, joint two and joint three respectively, and the mechanical arm comprises a front arm 5 and a rear arm 4; the first joint is driven by the first driving component 1 to realize the up-and-down swing of the mechanical arm, and the second joint is driven by the second driving component 2 to realize the left-and-right rotation of the mechanical arm; the joint III is driven by the third driving component 3, so that the rear end of the front arm 5 rotates relative to the front end of the rear arm 4.
The three joints have respective movable ranges, wherein the first joint is driven by the first driving assembly 1 to realize the up-and-down swing of the mechanical arm, and the second joint is driven by the second driving assembly 2 to realize the left-and-right rotation of the mechanical arm; the third joint is driven by the third driving component 3, so that the rear end of the front arm 5 rotates relative to the front end of the rear arm 4, the moving range of the first joint is between-15 degrees and 55 degrees, the moving range of the second joint is between-45 degrees and 45 degrees, and the moving range of the third joint is between 20 degrees and 150 degrees. Each joint will take a sample every 5 deg., for example, joint one has a range of motion of 70 deg., and needs to be sampled 15 times from the initial angle. In step 1, the position scanning method is as follows (as shown in fig. 4):
returning each joint to an initial angle position;
then, gradually changing the angle positions of the joint from the initial angle at intervals of 5 degrees, and scanning the corresponding gravity moment at each angle position; each angular position of the first joint corresponds to a plurality of gravity moments of different angular positions of the second joint and the third joint;
when each angular position of the first joint is scanned, the second joint and the third joint are returned to the initial angular position, then the angular position of the third joint is gradually changed every 5 degrees until the angular position of the third joint exceeds the end angular position (150 degrees), the second joint is gradually changed every 5 degrees, and in each position of the second joint, the third joint moves from the initial angular position to the end angular position until the angular position of the second joint exceeds the end angular position (45 degrees), the first joint is gradually changed every 5 degrees until the angular position of the first joint exceeds the end angular position (55 degrees). Scanning at each position, correspondingly recording joint angles and gravity moments of each joint at different angle positions, carrying out gravity calibration, and finally storing the gravity calibration into a corresponding data storage unit.
The gravity compensation method is characterized in that the gravity compensation method is used for realizing the compensation of the mechanical arm on the gravity under the complex mechanical environment, the gravity calibration mode is adopted, the gravity of each mechanism under the influence of the gravity is scanned and calibrated, then the gravity compensation under the real-time angle is matched from stored data, the real-time resultant moment respectively measured by the three joints is subtracted by the gravity moment extracted from the data storage unit corresponding to the positions of the three joints, so that the external force moment respectively received by the three joints is obtained.
The external moment respectively born by the three joint points under the action of the arm of the rehabilitation person can be obtained through the gravity compensation operation, the external moment is converted into the joint output speed according to the control algorithm, the feedback of the dynamic moment is realized, the tail end speed is changed along with the change of the force of the arm and the direction, and the tail end is moved along with the active force of the patient.
In order to achieve the effect of real-time dynamic compensation torque of the mechanical arm before the target speed is not reached, the target torque should be set to zero. In the PID algorithm, a target value needs to be set, in the scheme, namely, the target moment, only the target moment is set to be zero, and the effect of the external force acting by the external force in each positive and negative direction is consistent.
According to the external torque, the output speed and the output torque of the mechanical arm at the next moment are calculated by using a position type PID control algorithm, and the calculation formula is as follows:
wherein ：for the output speed or output torque of the arm at the next moment, +.>Is a proportional coefficient->Setting a difference between the value and the current state for the user; />For the integral coefficient +.>Is the accumulation of errors; />As a result of the differential coefficient,the difference between the current error and the last error.
Each joint is provided with a torque sensor and an angle sensor, and when the position scanning is carried out in the step 1, the torque corresponding to each joint at different angles is recorded through the angle sensor and the torque sensor respectively; the real-time angle measured by the angle sensor in step 2; in the step 3, the torque sensor measures the real-time resultant moment of the three joints of the mechanical arm.
When the gravity calibration is carried out in the step 1, calibration sampling is carried out every 5 degrees instead of continuous calibration of all positions, so that storage space is saved on the premise of ensuring that gravity change has certain continuity.
The gravity calibration can be calibrated once when leaving the factory and stored in the corresponding data storage unit, calibration and calculation are not needed each time, the calibrated gravity moment can be extracted at any time later, and gravity calibration is needed again only when the gravity changes after the accessory is replaced or when the transmission causes of parts (such as aging of a transmission belt or abrasion of a gear) are caused.
When the gravity moment is extracted in the step 2, because the calibration density is adopted once every 5 degrees, the real-time angle can be between two adjacent calibration gravity moments, and the gravity moment corresponding to the real-time angle is calculated according to the two adjacent calibration gravity moments. In step 2, after extracting the heavy moment, the method for calculating the heavy moment corresponding to the real-time angle according to the extracted heavy moment is as follows:
assume that the real-time angle x is at the nominal angle x 0 and x1 The calculation formula is as follows:
wherein ：、/>、/>respectively is an angle x, x 0 and x1 Lower gravitational moment.
As shown in FIG. 2, the invention provides a gravity compensation control system of a three-dimensional upper limb rehabilitation training device, which is used for realizing the gravity compensation method of the three-dimensional upper limb rehabilitation training device and comprises a controller, an angle sensor, a torque sensor, a position scanning module, a data storage unit and a real-time measurement module.
The controller comprises an interpolation calculation module and a compensation calculation module, wherein the interpolation calculation module extracts the corresponding gravity moment in the data storage unit and calculates the gravity moment of the real-time angle; the compensation operation module obtains a real-time angle and calculates external moments respectively born by the three joints according to the gravity moment of the real-time angle and the real-time resultant moment of each joint, so as to perform gravity compensation;
the position scanning module is used for carrying out position scanning on the full-posture space of each joint, recording joint angles and gravity moments of each joint under different angles and carrying out gravity calibration;
the data storage unit is used for storing the gravity moment corresponding to the joint angle during gravity calibration so as to be extracted by the controller;
the real-time measuring module is used for measuring the position of the mechanical arm in real time to obtain a real-time angle for the controller to extract;
the angle sensor and the torque sensor record the angle and the torque of each joint at each position respectively.
The foregoing is illustrative of the best mode of carrying out the invention, and is not presented in any detail as is known to those of ordinary skill in the art. The protection scope of the invention is defined by the claims, and any equivalent transformation based on the technical teaching of the invention is also within the protection scope of the invention.
Claims (8)
1. A gravity compensation method of a three-dimensional upper limb rehabilitation training instrument is characterized by comprising the following steps of: comprises the steps of,
step 1, performing position scanning on the three joint full-posture spaces, recording joint angles and gravity moments of each joint under different angles, performing gravity calibration, and finally storing the gravity calibration into a corresponding data storage unit;
step 2, obtaining a real-time angle through measuring the position of the mechanical arm in real time, and then extracting the heavy moment from the corresponding data storage unit;
and 3, respectively measuring real-time resultant moment of the three joints of the mechanical arm, and carrying out gravity compensation operation according to the gravity moment extracted in the step 2.
2. The gravity compensation method of the three-dimensional upper limb rehabilitation training device according to claim 1, wherein the gravity compensation method comprises the following steps: in step 3, the operation formula of the gravity compensation operation is as follows:
, wherein ,/>、/>、/>The arm of the rehabilitee applies acting force to the tail end of the front arm of the mechanical arm for external moment respectively applied to the three joints, wherein the external moment is the force transmitted to each joint by the acting force, and the joint is a joint>、/>、/>For the weight moment extracted by the three joints from the data storage unit corresponding to the position of the joints, +.>、/>、/>The real-time resultant moment is obtained by respectively measuring three joints, and the resultant moment is the moment of the joints after the joints bear external moment and heavy moment.
3. The gravity compensation method of the three-dimensional upper limb rehabilitation training device according to claim 1, wherein the gravity compensation method comprises the following steps: and (3) when the gravity calibration is carried out in the step (1), carrying out calibration sampling at intervals of 5 degrees.
4. The gravity compensation method of the three-dimensional upper limb rehabilitation training device according to claim 1, wherein the gravity compensation method comprises the following steps: the three joints are respectively a first joint, a second joint and a third joint, and the mechanical arm comprises a front arm (5) and a rear arm (4);
the first joint is driven by the first driving component (1) to realize the up-and-down swing of the mechanical arm, and the second joint is driven by the second driving component (2) to realize the left-and-right rotation of the mechanical arm; the joint III is driven by a third driving component (3) to realize the rotation of the rear end of the front arm (5) relative to the front end of the rear arm (4).
5. The gravity compensation method of the three-dimensional upper limb rehabilitation training device according to claim 4, wherein the gravity compensation method comprises the following steps: in step 1, the method of the position scanning is as follows,
returning each joint to an initial angle position;
then, gradually changing the angle positions of the joint from the initial angle at intervals of 5 degrees, and scanning the corresponding gravity moment at each angle position; each angular position of the first joint corresponds to a plurality of gravity moments of different angular positions of the second joint and the third joint;
when each angle position of the joint I is scanned, the joint II and the joint III are returned to the initial angle positions, then the angle positions of the joint III are gradually changed every 5 degrees until the angle position of the joint III exceeds the end angle position, the joint II is gradually changed every 5 degrees, and in each position of the joint II, the joint III moves from the initial angle position to the end angle position until the angle position of the joint II exceeds the end angle position, the angle position of the joint I is gradually changed every 5 degrees until the angle position of the joint I exceeds the end angle position.
6. A method for gravity compensation of a three-dimensional upper limb rehabilitation training device according to claim 3, wherein: in step 2, after extracting the heavy moment, the method for calculating the heavy moment corresponding to the real-time angle according to the extracted heavy moment is as follows:
assume that the real-time angle x is at the nominal angle x 0 and x1 The calculation formula is as follows:, wherein ：/>、/>、/>Respectively is an angle x, x 0 and x1 Lower gravitational moment.
7. The gravity compensation method of the three-dimensional upper limb rehabilitation training device according to claim 1, wherein the gravity compensation method comprises the following steps: each joint is provided with a torque sensor and an angle sensor, and when the position scanning is carried out in the step 1, the torque corresponding to each joint at different angles is recorded through the angle sensor and the torque sensor respectively; in the step 2, measuring a real-time angle through an angle sensor; in the step 3, real-time resultant moment of three joints of the mechanical arm is measured through a torque sensor.
8. A gravity compensation control system of a three-dimensional upper limb rehabilitation training device, for implementing the gravity compensation method of the three-dimensional upper limb rehabilitation training device according to any one of claims 1 to 7, which is characterized in that:
the device comprises a controller, an angle sensor, a torque sensor, a position scanning module, a data storage unit and a real-time measurement module;
the controller comprises an interpolation calculation module and a compensation calculation module, wherein the interpolation calculation module extracts the corresponding gravity moment in the data storage unit and calculates the gravity moment of the real-time angle; the compensation operation module obtains a real-time angle and calculates external moments respectively born by the three joints according to the gravity moment of the real-time angle and the real-time resultant moment of each joint, so as to perform gravity compensation;
the position scanning module is used for carrying out position scanning on the full-posture space of each joint, recording joint angles and gravity moments of each joint under different angles and carrying out gravity calibration;
the data storage unit is used for storing the gravity moment corresponding to the joint angle during gravity calibration so as to be extracted by the controller;
the real-time measuring module is used for measuring the position of the mechanical arm in real time to obtain a real-time angle for the controller to extract;
the angle sensor and the torque sensor record the angle and the torque of each joint at each position respectively.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310735480.5A CN116492201B (en) | 2023-06-21 | 2023-06-21 | Gravity compensation method and control system of three-dimensional upper limb rehabilitation training instrument |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310735480.5A CN116492201B (en) | 2023-06-21 | 2023-06-21 | Gravity compensation method and control system of three-dimensional upper limb rehabilitation training instrument |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116492201A true CN116492201A (en) | 2023-07-28 |
| CN116492201B CN116492201B (en) | 2023-10-03 |
Family
ID=87323341
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310735480.5A Active CN116492201B (en) | 2023-06-21 | 2023-06-21 | Gravity compensation method and control system of three-dimensional upper limb rehabilitation training instrument |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116492201B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118161835A (en) * | 2024-05-13 | 2024-06-11 | 江西求是高等研究院 | Mechanical arm power-assisted control method, rehabilitation robot, computer and medium |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20150141246A (en) * | 2014-06-09 | 2015-12-18 | 고려대학교 산학협력단 | Robot manipulator device being capable of observing the frictional torque on the articulation of the same and a method for controlling the same |
| CN107397649A (en) * | 2017-08-10 | 2017-11-28 | 燕山大学 | A kind of upper limbs exoskeleton rehabilitation robot control method based on radial base neural net |
| CN109953761A (en) * | 2017-12-22 | 2019-07-02 | 浙江大学 | A kind of lower limb rehabilitation robot sensory perceptual system and motion intention inference method |
| CN113197752A (en) * | 2021-04-30 | 2021-08-03 | 华中科技大学 | Limb gravity dynamic compensation method of upper limb rehabilitation robot |
| CN114432663A (en) * | 2022-01-30 | 2022-05-06 | 中航创世机器人(西安)有限公司 | Six-degree-of-freedom upper limb rehabilitation robot and gravity compensation method thereof |
-
2023
- 2023-06-21 CN CN202310735480.5A patent/CN116492201B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20150141246A (en) * | 2014-06-09 | 2015-12-18 | 고려대학교 산학협력단 | Robot manipulator device being capable of observing the frictional torque on the articulation of the same and a method for controlling the same |
| CN107397649A (en) * | 2017-08-10 | 2017-11-28 | 燕山大学 | A kind of upper limbs exoskeleton rehabilitation robot control method based on radial base neural net |
| CN109953761A (en) * | 2017-12-22 | 2019-07-02 | 浙江大学 | A kind of lower limb rehabilitation robot sensory perceptual system and motion intention inference method |
| CN113197752A (en) * | 2021-04-30 | 2021-08-03 | 华中科技大学 | Limb gravity dynamic compensation method of upper limb rehabilitation robot |
| CN114432663A (en) * | 2022-01-30 | 2022-05-06 | 中航创世机器人(西安)有限公司 | Six-degree-of-freedom upper limb rehabilitation robot and gravity compensation method thereof |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118161835A (en) * | 2024-05-13 | 2024-06-11 | 江西求是高等研究院 | Mechanical arm power-assisted control method, rehabilitation robot, computer and medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116492201B (en) | 2023-10-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104698848B (en) | A kind of control method of lower limb exoskeleton rehabilitation robot rehabilitation training | |
| CN110000787B (en) | Control method of super-redundant mechanical arm | |
| CN105437209B (en) | A kind of dermaskeleton type upper limb rehabilitation robot with human-computer interaction kinetic model | |
| JP5438290B2 (en) | Golf club swing simulation method | |
| US10183399B2 (en) | Six-axis motion mechanism | |
| CN103869834B (en) | Three-axis air-bearing table barycenter intelligent adjusting method based on empirical modal method | |
| CN113977602A (en) | Force feedback tail end holder admittance control method | |
| CN106983589A (en) | A kind of active above-knee prosthesis TSM control method based on interference observer | |
| CN112215172A (en) | Human body prone position three-dimensional posture estimation method fusing color image and depth information | |
| CN116492201B (en) | Gravity compensation method and control system of three-dimensional upper limb rehabilitation training instrument | |
| CN114886567B (en) | Surgical robot hand-eye calibration method oriented to constraint of telecentric fixed point | |
| CN112472531A (en) | Gait smoothing algorithm of lower limb exoskeleton robot for medical rehabilitation and assisted walking | |
| CN112237524A (en) | Control method for passive rehabilitation training of flexible upper limb rehabilitation robot | |
| Bailly et al. | Estimating the center of mass and the angular momentum derivative for legged locomotion—a recursive approach | |
| CN114055461A (en) | Robot force and position synchronous teleoperation control method and device based on myoelectric interface | |
| CN110900608B (en) | Robot kinematics calibration method based on optimal measurement configuration selection | |
| CN208591046U (en) | The detection device of unstable motion data | |
| CN114028156B (en) | Rehabilitation training method and device and rehabilitation robot | |
| CN113650014B (en) | Redundant mechanical arm tracking control method based on echo state network | |
| CN111507920B (en) | Bone motion data enhancement method and system based on Kinect | |
| CN108742840B (en) | Calibration method and device for robot | |
| Zheng et al. | Kinematic characteristics analysis of wearable lower limb rehabilitation exoskeleton robot based on Adams | |
| CN114434452B (en) | Mirror image mechanical arm control method based on potential energy field and mirror image mechanical arm equipment | |
| CN114948586B (en) | Human-machine system kinematics parameter identification and control method of lower limb rehabilitation robot | |
| CN115519582A (en) | Leg simulation exercise table for testing knee joint exoskeleton |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |