Summary of the invention
In order to solve the deficiencies in the prior art, the present invention provides the controls of master-slave mode upper limb exoskeleton rehabilitation robot to be
System, robot drive suffering limb that principal arm is followed to be good for limb movement, reach suffering limb and strong limb cooperative motion effect from arm executing agency.
Master-slave mode upper limb exoskeleton rehabilitation robot control system, comprising:
Sensor module, joint when the sensor module detection master-slave mode upper limb exoskeleton rehabilitation robot works
Principal and subordinate's arm posture information, characterize patient motion intention information and prevent from believing from the off-limits switch of each joint motions of arm
Breath;
Basic motion controller, receiving sensor module various information detected are simultaneously uploaded to upper layer main control computer;
Upper layer main control computer is handled to obtain control moment by the data that sensor module obtains, forms control
Instruction is issued to basic motion controller, and control robot is moved from arm driving mechanism, so that robot be driven to hold from arm
Row mechanism completes corresponding actions.
Further, the sensor module includes each joint for being mounted on master and slave arm executing agency of robot
Angular displacement sensor and velocity sensor, the position for detecting each joint of principal and subordinate's arm in training process obtain posture information;
It is mounted on the pressure sensor of each joint of principal arm executing agency of robot, is good for limb contact for detecting patient
Power obtains the motion intention of patient;And
Robot is mounted on from each joint of arm executing agency close to switch, for preventing from each joint motions of arm
It goes beyond the scope.
Further, above-mentioned master-slave mode upper limb exoskeleton rehabilitation robot control system further includes physical signs detection mould
Block, for patient in the training process physical signs and be uploaded to upper layer main control computer in real time, when physical signs is more than just
Normal range, upper layer main control computer assign halt instruction, deconditioning to basic motion controller.
Further, the robot principal arm of the master-slave mode upper limb exoskeleton rehabilitation robot, robot execute machine from arm
For structure respectively there are five freedom degree, respectively shoulder portion flexion/extension joint, shoulder inside/outside revolves joint, ancon flexion/extension joint, ancon
Inside/outside revolves joint and wrist joint flexion/extension joint.
Further, the robot of the master-slave mode upper limb exoskeleton rehabilitation robot is to drive machine from arm driving mechanism
From the driving motor of arm executing agency, the quantity of motor and the quantity in joint correspond people.
Disclosed herein as well is master-slave mode upper limb exoskeleton rehabilitation robot control methods, comprising:
Angle signal, the speed signal when each joint motions of principal and subordinate's arm are obtained, is pressed when obtaining each joint motions of principal arm
Approach signal when force signal and joint motions each from arm;
The signal of above-mentioned acquisition is uploaded to upper layer main control computer by WLAN by basic motion controller, with this
Physical signs when life is by patient motion simultaneously is uploaded to upper layer main control computer;
Upper layer main control computer obtains principal and subordinate's arm pose by each joint angles signal of principal and subordinate's arm, is judged by speed signal
Principal and subordinate's arm pose departure degree and judge from arm whether within the scope of safe speed at present, sentences from each joint of arm close to switching signal
Disconnected whether to exceed scope of activities from arm, whether in safe range, by principal arm pressure signal patient's physical signs calculates master
Arm motion intention forms control instruction to calculate from arm control moment;
It is formed by under control instruction and reaches basic motion controller, basic motion controller drives robot to drive from arm
Mechanism is formed by instruction movement according to upper layer main control computer;
Robot moves band mobile robot according to instruction from arm driving mechanism and moves from arm executing agency.
Further, the upper layer main control computer obtains principal and subordinate's arm pose by each joint angles signal of principal and subordinate's arm:
Upper layer main control computer obtains each joint angles of principal and subordinate's arm by angular displacement sensor, measures each joint position of principal arm
It sets, obtains each joint position of principal arm;
Principal arm pose obtains calculating to obtain from arm expected pose by inverse solution from arm expected pose by space reflection
From each joint expected pose point of arm and expected angle, wherein from arm expected angle with currently compared from each joint angles of arm,
Obtain angle difference
It obtains after each joint expected pose point of arm, advise from arm track with the current pose point comparison from each joint of arm
It draws, it would be desirable to which pose point and current pose point bring cubic spline function into, using cubic spline function to current inter-two-point path
It is planned, makes to move from arm steady.
Further, principal arm motion intention, detailed process are calculated by principal arm pressure signal are as follows:
The paired pressure sensor of each joint setting of principal arm is good in the dynamic principal arm motion process of limb girdle in patient to be formed inwardly
Contact force and outside contact force, the difference that the final reciprocal force in final each joint both is, direction by the two it is biggish that
A decision;
Principal arm motion intention is quantified after obtaining each joint reciprocal force of principal arm, due to the installation of principal arm pressure sensor
Position is fixed, so obtained reciprocal force is also fixed value to respective joint rotary shaft distance, utilizes the interaction force signal measured
It can be used as principal arm motion intention with the torque being multiplied to rotary shaft distance;
Further, it using principal arm motion intention, calculates from arm control moment, forms control instruction, specifically:
Design fuzzy close system, by quantifying after interaction torque between upper limb and healing robot principal arm form, structure
Fuzzy system is made, for the input variable of each freedom degree, ambiguity in definition set constructs system using rule:
It selects the product inference machine center method of average to carry out anti fuzzy method obtained fuzzy rule, obtains the output of system;
After obtaining fuzzy close system, control law is set, final control moment is obtained.
Further, upper layer main control computer real-time monitoring patient's physical signs, when blood oxygen saturation or heart rate exceed
Normal range (NR) assigns halt instruction to basic motion controller, and robot shutdown is awaited orders.
Further, suffering limb shape is embodied from each joint velocity variation of arm and motor torque variation by detection healing robot
State, constructed fuction e=δ+T, wherein δ is the speed signal that detects of sensor module velocity sensor, T=9.55UI/n be from
Shoulder joint motor torque, U are electric moter voltage, and I is current of electric, and n is motor speed;
It when constructed fuction e is greater than default threshold value, sounds an alarm, robot shutdown is awaited orders.
Compared with prior art, the beneficial effects of the present invention are:
1, the present invention using using principal arm and from arm formed master-slave mode cooperate so that control robot from arm driving mechanism into
Row movement, to drive robot to complete corresponding actions from arm executing agency, for replacing therapist that patient is helped to carry out power-assisted
Training helps patient to restore upper limb movement function, hemiplegic patient is enable to complete some simple number of storage tanks produced per day, remolds it certainly
Confidence mitigates family's pressure.
2, set-up of control system of the invention has physical signs to detect and close to switch, and training movement can be held in patient
By within the scope of, from arm close to switch for preventing from going beyond the scope from each joint motions of arm, the secondary injury to suffering limb is prevented.
3, present invention employs the control algolithms of intelligence, and the accurate fortune from arm can be controlled according to the exercise data of detection
It is dynamic, reach better training effect.
4, healing robot of the present invention can be intended to that principal arm is followed to move according to patient motion from arm, can be effectively prevented and tremble
Dynamic malfunction, can avoid the secondary injury to suffering limb, the experimental results showed that, no big ups and downs smooth from arm motion profile, control rail
It is good that mark tracks principal arm effect.
Specific embodiment
It is noted that following detailed description is all illustrative, it is intended to provide further instruction to the application.Unless another
It indicates, all technical and scientific terms used herein has usual with the application person of an ordinary skill in the technical field
The identical meanings of understanding.
It should be noted that term used herein above is merely to describe specific embodiment, and be not intended to restricted root
According to the illustrative embodiments of the application.As used herein, unless the context clearly indicates otherwise, otherwise singular
Also it is intended to include plural form, additionally, it should be understood that, when in the present specification using term "comprising" and/or " packet
Include " when, indicate existing characteristics, step, operation, device, component and/or their combination.
Master-slave mode upper limb exoskeleton rehabilitation robot involved in examples of implementation in the application, specific structure such as Fig. 1 institute
Show, including pedestal 1, control cabinet 2, seat 3, robot principal arm, robot are from arm;Pedestal 1 is fixed on ground, and seat 3 is placed in base
1 front of seat, control cabinet 2 are placed in 1 rear portion of pedestal, opposite with seat 3;Robot principal arm, robot are mounted on pedestal top from arm,
Seat side, is robot principal arm on the right of seat side, and the left side is robot from arm;Robot principal arm, robot are distinguished from arm
There are five freedom degrees, are shoulder portion flexion/extension joint, shoulder inside/outside rotation joint, ancon flexion/extension joint, ancon inside/outside rotation joint
And wrist joint flexion/extension joint.
Specifically, robot principal arm includes principal arm wrist joint flexion/extension joint 4, principal arm ancon flexion/extension joint 5, principal arm ancon
Inside/outside revolves joint 6, principal arm shoulder flexion/extension joint 7, and principal arm shoulder inside/outside revolves joint 8;
Robot includes revolving joint 9 from arm shoulder inside/outside from arm executing agency, from arm shoulder flexion/extension joint 11, from elbow
Portion's inside/outside revolves joint 13, from elbow portion flexion/extension joint 15, from carpal joint flexion/extension joint 17;
Robot includes revolving servo motor from elbow portion inside/outside from arm shoulder flexion/extension servo motor 12 from arm driving mechanism
14, from elbow portion flexion/extension servo motor 16, from carpal joint flexion/extension servo motor 18, servo motor is revolved from arm shoulder inside/outside
10。
The embodiment of the present application is also specifically disclosed that a kind of control system of above-mentioned master-slave mode upper limb exoskeleton rehabilitation robot
System, as shown in Figure 2, comprising: upper layer main control computer 100, basic motion controller 200, robot from arm driving mechanism 300,
Robot is from arm executing agency 400, sensor module 500, energy module 600, physical signs detection module 700.
Patient is good for limb and dresses robot principal arm, and suffering limb dresses robot from arm.Strong limb girdle moves principal arm movement, sensor module
500 acquire principal arms, from arm motion information, transfer information to basic motion controller 200 by bus;Basic motion controller
Motion information is uploaded to upper layer main control computer 100 by TCP/IP by 200;Upper layer main control computer 100 handles motion information
And control instruction is formed, instruction is by reaching basic motion controller 200 under TCP/IP;Basic motion controller 200 receives
It is moved from arm driving mechanism 300 by instruction after control instruction by driver control robot, so that robot be driven to hold from arm
The movement of row mechanism 400 drives suffering limb movement.Patient wears physical signs detection module 700 in the training process, passes through TCP/IP
The real-time blood oxygen saturation of patient, heart rate are uploaded to upper layer main control computer 100 and monitor patient's states, when patient's physical signs is super
Upper layer main control computer 100 issues halt instruction when crossing setting value.Entire training process provides the energy by system energy module 600
It supports.
Robot principal arm there are five freedom degree, respectively principal arm shoulder flexion/extension joint 7, principal arm shoulder inside/outside rotation joint 8,
Principal arm ancon flexion/extension joint 5, principal arm ancon inside/outside rotation joint 6 and principal arm wrist joint flexion/extension joint 4;
Robot from arm include robot from arm driving mechanism and robot from arm executing agency.
Upper layer main control computer 100 passes through Wireless LAN TCP/IP and basic motion controller 200 and physical signs
Detection module 700 is connected, and for carrying out information exchange with the healing robot basic motion controller 200, records training letter
Breath, real-time monitoring patient's physical signs send control instruction to the healing robot basic motion controller 200, realize control
Algorithm processed;
Basic motion controller 200 is connected by WLAN with upper layer main control computer 100, and bus and sensing are passed through
Device module 500 is connected, and is connected with robot from arm driving mechanism 300 by driver;Basic motion controller 200 receives upper layer
The control instruction of main control computer 100, control robot is moved from arm driving mechanism, so that robot be driven to execute from arm
Mechanism 400 completes corresponding actions, at the same time, information detected by 200 receiving sensor module 500 of basic motion controller
It is uploaded to upper layer main control computer 100 and realizes information exchange function.
Robot includes revolving servo from arm shoulder flexion/extension servo motor 12, from arm shoulder inside/outside from arm driving mechanism 300
Motor 10 revolves servo motor 14 from elbow portion flexion/extension servo motor 16, from elbow portion inside/outside and watches from carpal joint flexion/extension
Take motor 18;Robot is connected by driver with basic motion controller 200 from arm driving mechanism, in basic motion controller
After 200 receive the instruction of upper layer main control computer 100, robot is controlled from arm driving mechanism by basic motion controller 200
It is moved, driving robot completes corresponding actions from arm executing agency.
Robot from arm executing agency 400 include from arm shoulder flexion/extension joint 11, from arm shoulder inside/outside rotation joint 9, from
Elbow portion flexion/extension joint 15 revolves joint 13 from elbow portion inside/outside and from carpal joint flexion/extension joint 17;The robot
From arm executing agency by driving of the robot from arm driving mechanism, completes the task that upper layer main control computer 100 is assigned and refer to
It enables, patient's suffering limb is driven to carry out rehabilitation exercise.
Sensor module 500 includes being mounted on principal and subordinate arm shoulder flexion/extension joint (7,11), principal and subordinate's arm shoulder inside/outside rotation pass
Save (8,9), principal and subordinate's elbow portion flexion/extension joint (5,15), principal and subordinate's elbow portion's inside/outside rotation joint (6,13) and principal and subordinate's carpal joint
The angular displacement sensor 501 and velocity sensor 502 in flexion/extension joint (4,17), principal arm shoulder flexion/extension joint (7), principal arm shoulder
Inside/outside revolves joint (8), principal arm ancon flexion/extension joint (5), principal arm ancon inside/outside rotation joint (6) and principal arm wrist joint flexion/extension
The pressure sensor 503 in joint (4), further include from arm shoulder flexion/extension joint (11), from arm shoulder inside/outside rotation joint (9), from
Elbow portion flexion/extension joint (15) revolves joint (13) and approaching from carpal joint flexion/extension joint (17) from elbow portion inside/outside
Switch 504;Each 501 velocity sensor of joint angular displacement sensor of principal and subordinate's arm is for detecting each pass of principal and subordinate's arm in training process
The position of section obtains posture information, and each joint pressure sensor 503 of principal arm, which is arranged in pairs, is good for limb contact force for detecting patient, with
This obtains the motion intention of patient, from arm close to switch 504 for preventing from going beyond the scope from each joint motions of arm, prevents to suffering limb
Secondary injury;Sensor information obtained is uploaded to upper layer main control computer 100 by basic motion controller 200, complete
Motion control arithmetic is realized at information exchange.
Specifically, when elbow joint bending, inner sensors measure reciprocal force by taking the bending and stretching of elbow joint as an exampleWhen stretching, extension
Elbow lateral sensor measures reciprocal forceFinal elbow joint reciprocal force size is the difference of the two, and direction is by the two
In it is biggish that decision, so to be arranged in pairs;Specific location is at wrist, ancon, shoulder joints activity.
Energy module 600 is as shown in figure 3, include 220V power supply, 24V Switching Power Supply and 5V Switching Power Supply, for upper layer
Main control computer 100, basic motion controller 200, robot are from arm driving mechanism 300, sensor module 500 and physical signs
Detection module 700 provides energy safeguard.
Physical signs detection module 700 is connected by TCP/IP with upper layer main control computer 100, in the training process
The blood oxygen saturation and heart rate of patient are detected, uploads physical signs in real time to upper layer main control computer 100, when physical signs is more than
Normal range (NR), upper layer main control computer 100 assign halt instruction, deconditioning to basic motion controller 200;
Embodiments herein is also specifically disclosed that a kind of control method of master-slave mode upper limb exoskeleton rehabilitation robot,
It includes the following steps, as shown in figure 4,
Step 1: patient is good for the principal arm of limb wearing master-slave mode upper limb exoskeleton rehabilitation robot, is good for limb movement and drives rehabilitation
The movement of robot principal arm, principal and subordinate's arm angular displacement sensor 501, the acquisition principal and subordinate's arm of velocity sensor 502 of sensor module 500 are each
Angle signal, the speed signal in a joint, each joint pressure sensor 503 of principal arm obtain pressure signal and approach from each joint of arm
Switch 504 obtains approach signal;
Step 2: the pressure signal in each joint of angle signal, speed signal, principal arm in each joint of principal and subordinate's arm of acquisition and
Upper layer main control computer is uploaded to by WLAN by basic motion controller 200 from each joint of arm close to switching signal
100, patient's physical signs is uploaded to upper layer main control computer 100 by physical signs detection module 700 at the same time;
Step 3: upper layer main control computer 100 obtains principal and subordinate's arm pose by each joint angles signal of principal and subordinate's arm, passes through speed
Whether degree signal judges current principal and subordinate's arm pose departure degree and judges from arm within the scope of safe speed, close from each joint of arm
Switching signal judges whether exceed scope of activities from arm, and in safe range whether patient's physical signs, believed by principal arm pressure
Number principal arm motion intention is calculated, to calculate from arm control moment, forms control instruction;
Step 4: control instruction is formed by by reaching basic motion controller 200, basic motion under WLAN
Controller 200 by robot from arm driving mechanism 300 slave arm shoulder flexion/extension servo motor 12, from arm shoulder inside/outside revolve
Servo motor 10, from elbow portion flexion/extension servo motor 16, from elbow portion inside/outside revolve servo motor 14 and from carpal joint bend/
It stretches servo motor 18 and is formed by instruction movement according to upper layer main control computer 100;
Step 5: robot from arm driving mechanism 300 according to instruction move band mobile robot from arm executing agency 400 from
Pass is revolved from arm shoulder inside/outside rotation joint 9, from elbow portion flexion/extension joint 15, from elbow portion inside/outside in arm shoulder flexion/extension joint 11
It saves 13 and is moved from carpal joint flexion/extension joint 17, and then drive suffering limb that principal arm is followed to be good for limb movement, reach suffering limb and strong limb
Cooperative motion effect.
In step 3, upper layer main control computer formation instruction process is as follows, as shown in Figure 5
Step 31: upper layer main control computer obtains each joint angles of principal and subordinate's arm by angular displacement sensor, measures principal arm
Each joint position obtains each joint position P of principal armmi,k+1(tk+1,Smi,k+1) (P in formulami,k+1For discrete point, Smi,k+1For principal arm pass
Mi is saved in sampling instant tk+1The displacement at place);
Each joint position of principal arm obtains articulation angle, each joint of robot when obtaining, by angular displacement sensor
Length is it is known that modeling normal solution according to D-H and obtaining position of each joint of principal arm based on origin;Each joint position of principal arm is by length of connecting rod
Degree, connecting rod reverse, connecting rod deviates and joint angles normal solution and obtain.
Because robot is homotype isomorphism thus, after principal arm position determines, also can determine from arm position by Descartes's mapping.
Step 32: principal arm pose Pmi,k+1(tk+1,Smi,k+1) map to obtain from arm expected pose, such as by cartesian space
Shown in Fig. 6, is calculated and obtained from each joint expected pose point P of arm by the inverse solution of D-H algorithm from arm expected posesi,k+1(tk+1,
Ssi,k+1) (S in formulasi,k+1For from shoulder joint si in sampling instant tk+1The displacement at place) (as shown in Figure 7) and expected angle qd,
Middle expected angle with currently compared from each joint angles q of arm, obtain angle difference
Step 33: obtaining expected pose point Psi,k+1(tk+1,Ssi,k+1) after, the present bit obtained is mapped with by Descartes
Appearance point Psi,k(tk,Ssi,k) progress is compared from arm trajectory planning, it would be desirable to pose point Psi,k+1(tk+1,Ssi,k+1) and current pose point
Psi,k(tk,Ssi,k) bring cubic spline function into
S (t)=At3+Bt2+Ct+D
Pre-planning is carried out to speed(vsi,kFor speed, T is the sampling period),
And then obtain equation group:
A, B, C, D are solved, and then current inter-two-point path is planned using cubic spline function, is obtained from arm two o'clock
Between movement position, obtain more accurate angular error with 32, make to move from arm steady.
Step 34: the paired pressure sensor of each joint setting of principal arm is good in the dynamic principal arm motion process of limb girdle in patient to be formed
The final reciprocal force of inside contact force m and outside contact force m ', final each joint are the difference M of the two, and direction is by the two
In it is biggish that decision;
Step 35: principal arm motion intention being quantified after obtaining each joint reciprocal force of principal arm, due to principal arm pressure sensing
The position of device installation is fixed (herein it should be noted that being only equipped with pressure at wrist, ancon and shoulder freedom of movement in principal arm
Force snesor;Do not installed from arm), thus obtained reciprocal force to respective joint rotary shaft distance be also fixed value, using measuring
The torque τ that is multiplied with to rotary shaft distance L of interaction force signal MmIt can be used as principal arm motion intention, expression (i=
1,2,3,4,5 be five joints of principal arm):
Step 36: design fuzzy close systemFriendship after by quantifying between upper limb and healing robot principal arm
Mutual torque τmComposition constructs fuzzy system such as:θ is self-adjusting parameter.Q is to close
Save angle, the first derivative of q is joint angular speed, the second dervative of q is joint angular acceleration, and the input of each freedom degree is become
AmountDefine jiA fuzzy setUsingRule constructs system
The product inference machine center method of average is selected to carry out anti fuzzy method obtained fuzzy rule, the output for obtaining system is
Step 37: obtaining fuzzy close systemAfterwards, design control law obtains final control moment, control law
Are as follows:
In formula:Respectively from shoulder joint amount angular displacement, joint angle speed displacement amount and joint angular acceleration
Displacement, D (q) ∈ Rn×nFor from arm inertia force matrix,Indicate robot system from arm centrifugal force and Ge Shi
Power, G (q) ∈ RnFor gravity item,For fuzzy close system by quantifying after between upper limb and healing robot principal arm
Interaction torque τmComposition;τ is the control moment of robot system, KD=diag (Ki), Ki0, i=1,2 ..., 5, W sgn (s) of >
For Lu BangxiangΛ is positive definite matrix,For angle error in tracking,(qdFor the expected angle of step 32).
In step 3, upper layer main control computer protection process from each joint limit switch of arm as shown in figure 8, guarantee each from arm
Range of motion causes secondary injury to patient's suffering limb without departing from restriction;100 real-time monitoring patient of upper layer main control computer
Physical signs assigns halt instruction, machine to basic motion controller 200 when blood oxygen saturation or heart rate exceed normal range (NR)
Device people shuts down and awaits orders;
Suffering limb state is embodied from each joint velocity variation of arm and motor torque variation by detection healing robot, constructs letter
Number e=δ+T, wherein δ is the speed signal that sensor module velocity sensor detects, T=9.55UI/n is from shoulder joint electricity
Machine torque, U are electric moter voltage, and I is current of electric, and n is motor speed.Constructed fuction is when calculating, for each speed signal
Calculate separately to obtain multiple functional values.When constructed fuction e be greater than default threshold value when, system sounds an alarm, robot shut down to
Life.
The foregoing is merely preferred embodiment of the present application, are not intended to limit this application, for the skill of this field
For art personnel, various changes and changes are possible in this application.Within the spirit and principles of this application, made any to repair
Change, equivalent replacement, improvement etc., should be included within the scope of protection of this application.