A kind of control method of wearable assistance exoskeleton limb mechanism of upper
Technical field
The present invention relates to robot field, particularly relate to a kind of control method of wearable assistance exoskeleton limb mechanism of upper.
Background technology
Soldiers often needs to bear weight to carry out growing distance walking or fighting, overweight load often can cause certain injury to soldier's health, under this background, need to develop a ectoskeleton equipment that can strengthen soldier's speed, strength and endurance in battlefield surroundings; In fields such as scientific investigation, fire-fighting rescues, scientific investigation personnel and fire-fighting rescue worker usually need long distance walking, bear weight, transport the wounded, Field Operational, mountain climbing expedition etc., and traditional wheeled vehicles is difficult to play a role at these special occasions.In addition, ectoskeleton also can be used to the cargo handling in warehouse, to alleviate the labour intensity of porter.The combination of ectoskeleton and people can adapt to non-structured environment, has fabulous flexibility, can complete the work of the handling of some complexity, and as being fighter plane handling guided missiles etc., this is that other handling facilities hardly match.The application of ectoskeleton in these fields plays very positive effect by these fields.In addition, aging is just at global spread, and ectoskeletal appearance not only can help some the elderlys to solve poor, the constant problem of walking of muscle power, and some also can be helped to lose the ability to act of people's recovered part of ability to act.The feature of assistance exoskeleton requires to cooperate with wearer under non-structure environment, this requires that researcher needs to solve the man-machine integration problem of hight coordinate under unstructuredness environment, comprise effective, reliable between humans and machines interaction problems, to the quick response problem of human motion intention, light, biomimetic features design flexibly, the safety issue etc. of man-machine system, these technical problems are also in the elementary stage of fumbling, and immature, also need to carry out deep research.
Summary of the invention
The object of the invention is for the deficiencies in the prior art, provide a kind of control method of wearable assistance exoskeleton limb mechanism of upper, the method is effective between humans and machines interaction problems, reliable, and has the advantages that to respond fast human motion intention.
In order to achieve the above object, the technical solution adopted in the present invention is as follows: a kind of control method of wearable assistance exoskeleton limb mechanism of upper, and described wearable assistance exoskeleton limb mechanism of upper comprises: left arm, right arm, backrest, real-time controller and motor driver; Wherein, described left arm is identical with right arm structure, is hinged on the both sides of backrest respectively; Motor driver is connected with real-time controller;
Described left arm and right arm include: large arm motor, driven cylindrical gear, shoulder joint rotary encoder, ball pivot, large arm, forearm motor, driven wheel of differential, elbow joint rotary encoder, multi-dimension force sensor, forearm, palm, initiatively roller gear, drive bevel gear, forearm bandage; Wherein, large arm motor is arranged on the side, upper end of backrest; The output shaft of large arm motor is fixedly connected with active roller gear; Driven cylindrical gear is hinged in backrest, and with active roller gear engaged transmission, shoulder joint rotary encoder is set in hinged place; One end of ball pivot is fixedly connected with driven cylindrical gear, and the other end is fixedly connected with large arm upper end; The upper end thereof of large arm lower end and forearm, arranges elbow joint rotary encoder in hinged place; Forearm motor is arranged in large arm, and drive bevel gear is fixedly connected with forearm motor output shaft; Driven wheel of differential is fixed on forearm, and with drive bevel gear engaged transmission; Lower end and the palm of forearm are hinged; Multi-dimension force sensor is arranged on forearm, and multi-dimension force sensor is connected with forearm bandage; Large arm motor is all connected with motor driver with forearm motor; Shoulder joint rotary encoder, elbow joint rotary encoder are all connected with real-time controller with multi-dimension force sensor;
The method comprises the steps:
(1) initialize the sampling period T of real-time controller, get the value of T between 10 to 20 milliseconds; Large arm and forearm are rotated to parallel position, now, initializes shoulder joint rotary encoder and elbow joint rotary encoder, the numerical value of shoulder joint rotary encoder and elbow joint rotary encoder is returned to zero; Meanwhile, multi-dimension force sensor is initialized;
(2) when large arm and forearm relatively rotate, the signal of the multi-dimension force sensor on forearm is gathered;
(3) the power F of multi-dimension force sensor contact point is converted to by the computing of real-time controller and communication module the speed v that this point expects;
v=K
vF
Wherein: F is the active force between people-machine that multi-dimension force sensor records, if
F
xfor the active force of x-axis, F
yfor the active force of y-axis, M
zfor the moment of z-axis; K
vfor diagonal matrix, K
v=diag (k
x, k
y, k
w), k
xfor the linear velocity gain parameter of x-axis, k
yfor the linear velocity gain parameter of y-axis, k
wfor the rotational angular velocity gain parameter of z-axis; V is the movement velocity of multi-dimension force sensor mounting points, if
V
xfor the linear velocity of x-axis, v
yfor the linear velocity of y-axis, w
zfor the rotational angular velocity of z-axis;
(5) the inverse matrix ω=J of Jacobian matrix is calculated
-1v, draws the desired speed ω of shoulder joint and elbow joint, then carries out integration to it, draws the expected angle matrix q of shoulder joint and elbow joint
d;
(6) real-time controller is by gathering the angle information q of computing shoulder joint rotary encoder and elbow joint rotary encoder, exports voltage signal u (t) controlling motor;
Wherein, e (t)=q
d(t)-q (t) q
dt () is for real-time controller is by gathering the expected angle matrix of the shoulder joint that draws of computing and elbow joint, the angle matrix that q (t) measures for shoulder joint and elbow joint corresponding rotation encoder; k
pfor proportionality coefficient, k
ifor integration time constant, k
dfor derivative time constant;
(7) voltage signal u (t) that step 6 obtains by motor driver is converted into the current signal controlling motor, large arm motor and forearm motor are according to the size of current signal, realize the control to large arm motor and the forearm motor anglec of rotation, and then realize the control to shoulder joint and the elbow joint anglec of rotation.
Compared with prior art, the invention has the beneficial effects as follows: the present invention is mainly for auxiliary under long-time heavy burden operating environment or enhancing people upper limbs heavy burden ability.Its dynamical system adopts the motor drive mode with the feature such as energy-conservation, stable, controllability is high, floor space is little, running rate is high, respond fast, easy to maintenance.Sensing system is mainly distributed in the positions such as large arm forearm and realizes comparatively effective, reliable man-machine interaction.Arrange compliant mechanism, adopt anthropomorphic mechanism design, wearable structural design adapts to human physiological structure, by be coupled realization and the human body coordinated movement of various economic factors with human upper limb locomotion joint.The PID control method of strong adaptive capacity can allow upper limbs still have good performance under various complex working condition, has response fast, the features such as followability is good.
Accompanying drawing explanation
Fig. 1 is global shape structural representation of the present invention;
Fig. 2 is shoulder joint partial enlarged drawing of the present invention;
Fig. 3 is elbow joint partial enlarged drawing of the present invention;
Fig. 4 is real-time controller control structure block diagram;
Fig. 5 is control flow chart of the present invention;
In figure, backrest 1, large arm motor 2, driven cylindrical gear 3, shoulder joint rotary encoder 4, ball pivot 5, large arm 6, forearm motor 7, driven wheel of differential 8, elbow joint rotary encoder 9, multi-dimension force sensor 10, forearm 11, palm 12, initiatively roller gear 13, drive bevel gear 14, forearm bandage 15.
Detailed description of the invention
Below in conjunction with drawings and Examples, the present invention is further illustrated.
As Figure 1-3, a kind of wearable assistance exoskeleton limb mechanism of upper comprises: left arm, right arm, backrest 1, real-time controller, motor driver; Wherein, described left arm is identical with right arm structure, is hinged on the both sides of backrest 1 respectively; Motor driver is connected with real-time controller;
Described left arm and right arm include: large arm motor 2, driven cylindrical gear 3, shoulder joint rotary encoder 4, ball pivot 5, large arm 6, forearm motor 7, driven wheel of differential 8, elbow joint rotary encoder 9, multi-dimension force sensor 10, forearm 11, palm 12, initiatively roller gear 13, drive bevel gear 14, forearm bandage 15; Wherein, large arm motor 2 is arranged on the side, upper end of backrest 1; The output shaft of large arm motor 2 is fixedly connected with active roller gear 13; Driven cylindrical gear 3 is hinged in backrest 1, and with active roller gear 13 engaged transmission, shoulder joint rotary encoder 4 is set in hinged place; One end of ball pivot 5 is fixedly connected with driven cylindrical gear 3, and the other end is fixedly connected with large arm 6 upper end, can realize inside and outside stretching, extension one degree of freedom; The upper end thereof of large arm 6 lower end and forearm 11, arranges elbow joint rotary encoder 9 in hinged place; Forearm motor 7 is arranged in large arm 6, and drive bevel gear 14 is fixedly connected with forearm motor 7 output shaft; Driven wheel of differential 8 is fixed on forearm 11, and with drive bevel gear 14 engaged transmission; Lower end and the palm 12 of forearm 11 are hinged; Multi-dimension force sensor 10 is arranged on forearm 11, and multi-dimension force sensor 10 is connected with forearm bandage 15; Large arm motor 2 is all connected with motor driver with forearm motor 7; Shoulder joint rotary encoder 4, elbow joint rotary encoder 9 are all connected with real-time controller with multi-dimension force sensor 10;
As shown in Figure 4, described real-time controller comprises computing and communication module, data acquisition module and control output module; Wherein, described computing and communication module comprise CPU, network service, FPGA; CPU is connected by netting twine network interface card with network service, CPU with FPGA is connected by pci bus; Described data acquisition module comprises digital input module and analog input module; Described control output module is analog output module; The digital output port of digital input module is connected with the digital input port of FPGA, and the modulating output port of analog input module is connected with the analog input port of FPGA, and the input port of analog output module is connected with the modulating output port of FPGA; Shoulder joint rotary encoder 4 is connected with the shoulder joint data acquisition port of digital input module, and elbow joint rotary encoder 9 is connected with the elbow joint data acquisition port of digital input module; Two multi-dimension force sensors are connected with two multi-dimension force sensor data acquisition ports of analog input module respectively; The voltage output end mouth of analog output module is connected with motor driver; Large arm motor 2 is all connected with motor driver with the current input terminal mouth of forearm motor 7, and this voltage signal is converted into the current signal controlling motor by motor driver; Real-time controller is connected with host computer by Ethernet; The adoptable model of described real-time controller is the product of NIcRIO-9031, but is not limited thereto; The adoptable model of described motor driver is the product of EPOS2, but is not limited thereto.
A control method for wearable assistance exoskeleton limb mechanism of upper, as follows: when people has dressed assistance exoskeleton limb mechanism of upper, real-time controller receives the signal that the multi-dimension force sensor 10 on forearm 11 transmits.The power of multi-dimension force sensor contact point is converted to by the computing of real-time controller and communication module the speed that this point expects, then the inverse matrix of Jacobian matrix is calculated, draw the desired speed of shoulder joint and elbow joint, finally carry out the expected angle that integration draws shoulder joint and elbow joint.Real-time controller is by gathering the angle signal of computing shoulder joint rotary encoder 4 and elbow joint rotary encoder 9, thus export the voltage signal controlling large arm motor 2 and forearm motor 7, thus realize the Angle ambiguity to large arm motor 2 and forearm motor 7 output shaft.The rotary motion of motor, by the gear mechanism at shoulder joint and elbow joint place, is converted to the rotary motion of shoulder joint and elbow joint by motor output shaft.Shoulder joint rotary encoder 4 and elbow joint rotary encoder 9 are arranged on the output shaft of shoulder joint and elbow joint respectively, the anglec of rotation of shoulder joint and elbow joint can be fed back to real-time controller; Real-time controller carries out contrast computing to the information that feedback is come, and controls the motion of large arm motor 2 and forearm motor 7 further, thus realizes real-time feedback control.
As shown in Figure 5, the invention provides a kind of control method of wearable assistance exoskeleton limb mechanism of upper, specifically comprise the steps:
(1) initialize the sampling period T of real-time controller, get the value of T between 10 to 20 milliseconds; Large arm 6 and forearm 11 are rotated to parallel position, now, initializes shoulder joint rotary encoder 4 and elbow joint rotary encoder 9, the numerical value of shoulder joint rotary encoder 4 and elbow joint rotary encoder 9 is returned to zero; Meanwhile, multi-dimension force sensor 10 is initialized;
(2) when large arm 6 and forearm 11 relatively rotate, the signal of the multi-dimension force sensor 10 on forearm 7 is gathered;
(3) the power F of multi-dimension force sensor 10 contact point is converted to by the computing of real-time controller and communication module the speed v that this point expects;
v=K
vF
Wherein: F is the active force between people-machine that multi-dimension force sensor 10 records, if
F
xfor the active force of x-axis, F
yfor the active force of y-axis, M
zfor the moment of z-axis; K
vfor diagonal matrix, K
v=diag (k
x, k
y, k
w), k
xfor the linear velocity gain parameter of x-axis, k
yfor the linear velocity gain parameter of y-axis, k
wfor the rotational angular velocity gain parameter of z-axis; V is the movement velocity of multi-dimension force sensor 10 mounting points, if
V
xfor the linear velocity of x-axis, v
yfor the linear velocity of y-axis, w
zfor the rotational angular velocity of z-axis;
(5) the inverse matrix ω=J of Jacobian matrix is calculated
-1v, draws the desired speed ω of shoulder joint and elbow joint, then carries out integration to it, draws the expected angle matrix q of shoulder joint and elbow joint
d;
(6) real-time controller is by gathering the angle information q of computing shoulder joint rotary encoder 4 and elbow joint rotary encoder 9, exports voltage signal u (t) controlling motor;
Wherein, e (t)=q
d(t)-q (t) q
dt () is for real-time controller is by gathering the expected angle matrix of the shoulder joint that draws of computing and elbow joint, the angle matrix that q (t) measures for shoulder joint and elbow joint corresponding rotation encoder; k
pfor proportionality coefficient, k
ifor integration time constant, k
dfor derivative time constant;
(7) voltage signal u (t) that step 6 obtains by motor driver is converted into the current signal controlling motor, large arm motor 2 and forearm motor 7 are according to the size of current signal, realize the control to large arm motor 2 and forearm motor 7 anglec of rotation, and then realize the control to shoulder joint and the elbow joint anglec of rotation.