[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN112089580B - Lower limb skeleton rehabilitation robot motion control method based on interference compensation - Google Patents

Lower limb skeleton rehabilitation robot motion control method based on interference compensation Download PDF

Info

Publication number
CN112089580B
CN112089580B CN202010404277.6A CN202010404277A CN112089580B CN 112089580 B CN112089580 B CN 112089580B CN 202010404277 A CN202010404277 A CN 202010404277A CN 112089580 B CN112089580 B CN 112089580B
Authority
CN
China
Prior art keywords
knee joint
hip joint
joint
hip
knee
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.)
Active
Application number
CN202010404277.6A
Other languages
Chinese (zh)
Other versions
CN112089580A (en
Inventor
支岳
李古强
宫健伟
肖晓飞
王冉冉
孟永春
廖晨歌
宋晓慧
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Binzhou Medical College
Original Assignee
Binzhou Medical College
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Binzhou Medical College filed Critical Binzhou Medical College
Priority to CN202010404277.6A priority Critical patent/CN112089580B/en
Publication of CN112089580A publication Critical patent/CN112089580A/en
Application granted granted Critical
Publication of CN112089580B publication Critical patent/CN112089580B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • AHUMAN NECESSITIES
    • A61MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61HPHYSICAL 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
    • A61H3/00Appliances for aiding patients or disabled persons to walk about
    • AHUMAN NECESSITIES
    • A61MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61HPHYSICAL 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
    • A61H3/00Appliances for aiding patients or disabled persons to walk about
    • A61H2003/005Appliances for aiding patients or disabled persons to walk about with knee, leg or stump rests
    • AHUMAN NECESSITIES
    • A61MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61HPHYSICAL 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/00Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/50Control means thereof
    • A61H2201/5007Control means thereof computer controlled
    • AHUMAN NECESSITIES
    • A61MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61HPHYSICAL 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/00Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/50Control means thereof
    • A61H2201/5058Sensors or detectors
    • A61H2201/5061Force sensors
    • AHUMAN NECESSITIES
    • A61MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61HPHYSICAL 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/00Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/50Control means thereof
    • A61H2201/5058Sensors or detectors
    • A61H2201/5069Angle 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)
  • Manipulator (AREA)
  • Rehabilitation Tools (AREA)

Abstract

The invention relates to a lower limb skeleton rehabilitation robot based on interference compensation, and belongs to the field of rehabilitation robot design. The load moments of the hip joint and the knee joint are estimated by measuring the stress modes of the hip joint and the knee joint, and a friction force estimation model is established for compensation; meanwhile, the rotation angles of the hip joint and the knee joint are measured and compared with the expected gait to obtain an error angle signal, and then the nominal system of the rehabilitation robot is compensated in an equivalent control mode. And finally, constructing an uncertainty and interference observer of the robot system through the equivalent control quantity and the error angle of the nominal system, observing and compensating the system uncertainty of the hip joint and the knee joint, forming hip joint and knee joint control moment signals, and transmitting the signals to the rehabilitation robot system to realize the auxiliary walking control of the rehabilitee. The robot has the advantage that the robot can compensate various uncertainties, so that the overall load change resistance is high.

Description

Lower limb skeleton rehabilitation robot motion control method based on interference compensation
Technical Field
The invention relates to the field of motion control of lower limb skeleton rehabilitation robots, in particular to a motion control method of a lower limb skeleton rehabilitation robot based on interference compensation.
Background
With the development of society, the population structure of China has an obvious aging trend at present. A large number of elderly people have hemiplegia-like cardiovascular and cerebrovascular diseases, while most stroke or other patients have different degrees of lower limb movement disorders. With the development of the rehabilitation robot technology, the rehabilitation robot is widely adopted for the early-stage exercise rehabilitation treatment of the above cases to assist the patient in exercising, so that the aims of increasing the exercise amount of the rehabilitee, improving the limb exercise function of the patient and accelerating the rehabilitation process can be fulfilled; on the other hand, the device can provide convenience for the patients to take care of themselves in basic life nursing, reduce nursing burden and improve the life quality of the rehabilitees. The difficulty problems existing in the motion control of the lower limb skeleton rehabilitation robot are that the accurate measurement of the motion load is difficult, the estimation of the friction torque is inaccurate in the actual process, and unknown uncertain external interference exists in the motion control. Under the condition of strong uncertainty, the stability and the safety of rehabilitation motion control are ensured, the comfort level of the rehabilitation motion control is improved, and the rehabilitation motion control is a main task of the motion control. Based on the reasons, the invention provides a control scheme of interference observation compensation aiming at three uncertainties, and adopts a method of error feedback and equivalent control to realize high-quality control of the rehabilitation robot, so that the invention has good economic value and practical value.
It is to be noted that the information invented in the above background section is only for enhancing the understanding of the background of the present invention, and therefore, may include information that does not constitute prior art known to those of ordinary skill in the art.
Disclosure of Invention
The invention aims to provide a motion control method of a lower limb skeleton rehabilitation robot based on interference compensation, and further solves the problem of poor motion control comfort caused by insufficient anti-interference and uncertain load self-adaption capability due to the limitations and defects of the related technology at least to a certain extent.
According to one aspect of the invention, a motion control method of a lower limb bone rehabilitation robot based on interference compensation is provided, and comprises the following steps:
step S10, respectively mounting FUTEK LSB200 type force sensors on a thigh rod and a shank rod of the lower limb skeleton rehabilitation robot, measuring load forces of a hip joint and a knee joint, and respectively estimating load moments of the hip joint and the knee joint according to the positions of the sensors;
step S20, respectively installing incremental orthogonal photoelectric encoders on a thigh rod and a shank rod of the lower limb skeletal rehabilitation robot, measuring a hip joint rotation angle and a knee joint rotation angle of the skeletal robot, obtaining a hip joint angle error and a knee joint angle error according to motion data of human gait, and respectively integrating to obtain error integral signals;
s30, constructing a nonlinear filtering differentiator according to the measurement signals of the hip joint rotation angle and the knee joint rotation angle to obtain a hip joint angular rate signal and a knee joint angular rate estimation signal;
s40, measuring the weight of a thigh support rod and the weight of a shank support rod, constructing a skeleton system angular acceleration matrix and an angular velocity matrix according to the hip joint angle measurement value and the knee joint angle measurement value, and the length of the thigh support rod and the length of the shank support rod, and performing inverse transformation to obtain a skeleton system angular acceleration inverse matrix;
s50, calculating an angular velocity equivalent control quantity according to the hip joint rotation angular velocity estimation value and the knee joint rotation angular velocity estimation value, and calculating a thigh rod gravity related quantity and a shank rod gravity related quantity according to physical structure data of the rehabilitation robot;
step S60, compensating and designing the friction force of the hip joint and the friction force of the knee joint according to the estimated value of the rotation angle rate of the hip joint and the knee joint, and then calculating the equivalent control quantity of the hip joint and the knee joint according to the estimated value of the load moment of the hip joint and the knee joint and the inverse matrix of a bone angular acceleration system;
step S70, constructing a hip joint and knee joint interference observer according to the equivalent control quantity of the hip joint and the knee joint, and respectively calculating the states and the interference estimation values of the hip joint and knee joint interference observer;
and S80, carrying out linear combination according to the hip joint and knee joint interference estimated value, the hip joint and knee joint rotation angle error amount, the error integral amount and the hip joint and knee joint rotation angle rate signal estimated value to generate hip joint and knee joint control moment, and realizing the final motion control of the lower limb skeleton rehabilitation robot.
In an exemplary embodiment of the present invention, measuring a hip joint rotation angle and a knee joint rotation angle of the skeletal robot, obtaining a hip joint angle error and a knee joint angle error according to motion data of human gait, and integrating the hip joint angle error and the knee joint angle error respectively to obtain an error integration signal includes:
q d1 =a 11 sin(b 11 p+c 11 )+a 12 sin(b 12 p+c 12 )+a 13 sin(b 13 p+c 13 );
q d2 =a 21 sin(b 21 p+c 21 )+a 22 sin(b 22 p+c 22 )+a 23 sin(b 23 p+c 23 );
p=t/T aa -floor(t/T aa );
e 1 =q 1 -q d1
e 2 =q 2 -q d2
s 1 =∫e 1 dt;
s 2 =∫e 2 dt;
wherein q is 1 Measurement of hip joint rotation angle for a lower extremity skeletal rehabilitation robot, q 2 The measured value is the knee joint rotation angle of the lower limb skeleton rehabilitation robot. q. q.s d1 As desired value of hip joint rotation angle, q d2 Is a desired value of the knee joint rotation angle, a 11 、a 12 、a 13 、a 21 、a 22 、a 23 、b 11 、b 12 、b 13 、b 21 、b 22 、b 23 、c 11 、c 12 、c 13 、c 21 、c 22 、c 23 The detailed design of the human body is shown in the embodiment of the later-written case, which is gait data. p is the percentage of the gait cycle, T is the locomotion time of the rehabilitee, T aa The average period of the exercise steps of the rehabilitee. floor (T/T) indicates that integer parts are taken to the left, such as floor (3.5) =3, and finally the obtained p is more than or equal to 0 and less than or equal to 1. e.g. of the type 1 For hip joint angle error signals, e 2 Is knee joint angle error signal, s 1 The signal is integrated for the hip joint rotation angle error, where dt represents the integration of the time signal. s 2 The knee joint rotation angle error integral signal is obtained.
In an exemplary embodiment of the present invention, constructing a nonlinear filter differentiator according to the hip rotation angle and knee rotation angle measurement signals, and obtaining a hip angular rate signal and a knee angular rate estimation signal comprises:
Figure GDA0003990282560000041
D 1 (n)=(q 1 (n)-q 1a (n))/(T a |q 1 (n)-q 1a (n)|+ε 1 );
Figure GDA0003990282560000042
D 2 (n)=(q 2 (n)-q 2a (n))/(T b |q 2 (n)-q 2a (n)|+ε 2 );
wherein q is 1 For measuring signals of angle of rotation of the hip joint, D 1 Estimating a signal for the angular velocity of rotation of the hip joint, q 1a (n) is a filtered hip joint rotation angle signal, T a 、ε 1 、T b 、ε 2 The detailed design of the parameter is described in the following examples. q. q.s 2 For measuring knee joint rotation angle, D 2 Estimating a signal for knee joint rotational angular velocity, q 2a And (n) is a filtering knee joint rotation angle signal, and T is a time interval between data.
In an exemplary embodiment of the present invention, constructing the bone system angular acceleration matrix and the angular velocity matrix and the inverse matrix thereof according to the hip joint angle measurement value and the knee joint angle measurement value, and the length of the thigh strut and the length of the calf strut comprises:
Figure GDA0003990282560000043
Figure GDA0003990282560000044
Figure GDA0003990282560000045
Figure GDA0003990282560000046
Figure GDA0003990282560000047
M 0 M=E;
Figure GDA0003990282560000048
Π 11 =-m 2 l 1 l 2 cos(q 2 )D 2
Π 12 =-m 2 l 1 l 2 cos(q 2 )D 2 /2;
Π 21 =-m 2 l 1 l 2 cos(q 2 )D 2 /2;
Π 22 =0;
Figure GDA0003990282560000051
wherein m is 1 The weight of the thigh strut, m 2 Is the weight of the shank strut, M is the skeletal system angular acceleration matrix, M 0 Is the inverse matrix of the angular acceleration of the skeletal system, and E is the unit matrix. C is the bone system angular velocity matrix.
In an exemplary embodiment of the present invention, the calculating the angular velocity equivalent control quantity according to the hip joint rotational angular velocity estimation value and the knee joint rotational angular velocity estimation value, and then calculating the thigh bar gravity related quantity and the shank bar gravity related quantity according to the physical structure data of the rehabilitation robot includes:
Figure GDA0003990282560000052
g 1 =-m 1 gl 1 sin(q 1 )/2-m 2 gl 2 sin(q 1 -q 2 )/2-m 2 gl 1 sin(q 1 );
g 2 =-m 2 gl 2 sin(q 1 -q 2 )/2;
wherein H 0a For angular velocity equivalent control quantity, D 1 Estimating a signal for said angular velocity of rotation of the hip joint, D 2 Estimating a signal for said knee joint rotational angular velocity, h 0a1 Is the angular velocity equivalent control quantity of the hip joint, h 0a2 Is the knee joint angular velocity equivalent control quantity, g 1 And g 2 G is a gravity acceleration constant, and the value of g is 9.8.
In an exemplary embodiment of the present invention, the designing of the compensation of the friction force of the hip joint and the friction force of the knee joint according to the estimated values of the rotational angle rates of the hip joint and the knee joint, and the calculating the equivalent control quantities of the hip joint and the knee joint comprises:
f a1 =f s λ 1 +f c (1-λ 1 );
f a2 =f s λ 2 +f c (1-λ 2 );
Figure GDA0003990282560000053
Figure GDA0003990282560000061
M f1 =l a1 T 1
M f2 =l a2 T 2
Figure GDA0003990282560000062
wherein T is 1 And T 2 Measured values of the load forces of the hip and knee joints, respectively,/ a1 Is the distance between the hip joint and the installation position of the hip load force sensor, l a2 Is the distance between the knee joint and the installation position of the knee load force sensor. D 1 And D 2 Is an estimate of the angular velocity of rotation of the hip and knee joints, f a1 And f a2 The compensation amount of the friction force between the hip joint and the knee joint, f s And f c Are estimated values of the static friction coefficient and the kinetic friction coefficient, respectively. M f1 And M f2 For hip and knee load moment estimates, H 0a For equivalent control quantity of angular velocity, g 1 And g 2 The thigh bar gravity related quantity and the shank bar gravity related quantity u 1e And u 2e And finally calculating the equivalent control quantity of the hip joint and the knee joint.
In an exemplary embodiment of the present invention, constructing a hip joint and knee joint disturbance observer according to the equivalent control quantity of the hip joint and knee joint, and calculating the state and disturbance estimation values of the hip joint and knee joint disturbance observer respectively includes:
Figure GDA0003990282560000063
Figure GDA0003990282560000064
Figure GDA0003990282560000065
Figure GDA0003990282560000066
x 12d =k a1 D 1 +k b1 q 1
Figure GDA0003990282560000067
D 1d =u 1e -Y 110 u 1 -Y 120 u 2
Figure GDA0003990282560000068
Figure GDA0003990282560000069
Figure GDA00039902825600000610
Figure GDA0003990282560000071
x 22d =k a2 D 2 +k b2 q 2
Figure GDA0003990282560000072
D 2d =u 2e -Y 210 u 1 -Y 220 u 2
Figure GDA0003990282560000073
wherein k is a1 、k b1 、k a2 、k b2 The detailed design of the parameter is described in the following examples.
Figure GDA0003990282560000074
For hip joint rotation angle desired value q d1 The derivative of (c).
Figure GDA0003990282560000075
For knee joint rotation angle desired value q d2 The derivative of (c). u. of 1e For hip joint equivalent control quantity, u 1 Control of moment, u, for the hip joint 2e For knee joint equivalent control quantity, u 2 Controlling moment of knee joint, Y 110 、Y 120 、Y 210 、Y 220 For the bone angular acceleration system inverse matrix M 0 To finally obtain
Figure GDA0003990282560000076
As an estimate of hip joint interference, w 1a The state of the hip joint disturbance observer.
Figure GDA0003990282560000077
As knee joint disturbance estimate, w 2a Is the knee joint disturbance observer state.
In an exemplary embodiment of the present invention, the generating the hip and knee joint control torque according to the linear combination of the estimated hip and knee joint interference value, the error amount of the hip and knee joint rotation angle, the error integral amount, and the estimated hip and knee joint rotation angle rate signal value comprises:
Figure GDA0003990282560000078
Figure GDA0003990282560000079
wherein
Figure GDA00039902825600000710
And
Figure GDA00039902825600000711
respectively as said hip and knee joint interference estimates, e 1 And e 2 The difference between the rotation angle of hip joint and knee joint, s 1 And s 2 Are respectively the rotating angle error integral quantity u of the hip joint and the knee joint 1e And u 2e Respectively, the equivalent control quantity of hip joint and knee joint, D 1 And D 2 The estimated values of the rotation angular rate signals of the hip joint and the knee joint respectively 1 And u 2 For the resulting hip and knee control moment, k 21 、k 22 And k is 23 、k 11 、k 12 And k is 13 The detailed design of the parameter is described in the following examples.
The generated hip joint control moment and knee joint control moment are transmitted to a lower limb skeleton rehabilitation robot system, so that the rehabilitation person can be assisted to walk.
Advantageous effects
The lower limb skeleton rehabilitation robot motion control method based on interference compensation has the advantages that firstly, a method of an interference observer is adopted, so that load uncertainty, friction uncertainty and unknown interference uncertainty in the rehabilitation robot can be effectively estimated and compensated. Secondly, an equivalent uncertainty method is adopted, the nominal quantity of the rehabilitation robot is equivalently estimated, meanwhile, the load is measured and compensated, and the unknown friction torque is estimated and compensated. The compensation in the two aspects can greatly improve the dynamic effect of gait tracking control of the rehabilitation robot and improve the comfort of the rehabilitation person in assisting walking movement. Finally, the hip and knee motion angular rate of the rehabilitation robot is estimated in a nonlinear filtering differential mode, so that an angular rate measuring component is prevented from being installed, and the economic cost of the rehabilitation robot in production and manufacturing is reduced. Therefore, the invention has the advantages of economy, practicability and comfort, and has high engineering application and popularization values.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the invention, as claimed.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and together with the description, serve to explain the principles of the invention. It is obvious that the drawings in the following description are only some embodiments of the invention, and that for a person skilled in the art, other drawings can be derived from them without inventive effort.
FIG. 1 is a flow chart of a motion control method of a lower limb skeleton rehabilitation robot based on interference compensation provided by the invention;
FIG. 2 is a schematic view of a geometry of a lower limb skeletal rehabilitation robot according to a method provided by an embodiment of the invention;
FIG. 3 is a graph of expected hip joint rotation angle values (in degrees) in accordance with a method provided by an embodiment of the present invention;
FIG. 4 is a graph of hip rotation angle measurements (in degrees) in accordance with a method provided by an embodiment of the present invention;
FIG. 5 is a graph of measured hip joint rotation angle versus expected hip joint rotation angle (in degrees) according to a method provided by an embodiment of the present invention;
FIG. 6 is a graph of hip joint rotation error angle (in degrees) according to a method provided by an embodiment of the present invention;
FIG. 7 is a curve of expected values of knee joint rotation angles (in degrees) according to a method provided by an embodiment of the present invention;
FIG. 8 is a graph of measured knee joint rotation angle (in degrees) using a method provided by an embodiment of the present invention;
FIG. 9 is a graph of measured knee joint rotation angle versus expected value (in degrees) according to a method provided by an embodiment of the present invention;
FIG. 10 is a knee joint rotation error angle curve (unit: degree) according to a method provided by an embodiment of the present invention;
FIG. 11 is a hip joint rotational angular velocity estimation signal (in degrees/second) of a method provided by an embodiment of the present invention;
FIG. 12 is a signal (in degrees/second) of angular velocity estimation of knee joint rotation according to a method provided by an embodiment of the present invention;
FIG. 13 is a hip joint interference estimate (in Nm) for a method provided by an embodiment of the invention;
FIG. 14 is a knee joint disturbance estimate (in Nm) using the method of the present invention;
FIG. 15 is a hip joint control moment (in Nm) of a method provided by an embodiment of the present invention;
FIG. 16 shows knee joint control moment (unit: nm) of a method provided by an embodiment of the present invention.
Detailed Description
Example embodiments will now be described more fully with reference to the accompanying drawings. Example embodiments may, however, be embodied in many different forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the concept of example embodiments to those skilled in the art. The described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments. In the following description, numerous specific details are provided to provide a thorough understanding of embodiments of the invention. One skilled in the relevant art will recognize, however, that the invention can be practiced without one or more of the specific details, or with other methods, components, devices, steps, and so forth. In other instances, well-known technical solutions have not been shown or described in detail to avoid obscuring aspects of the invention.
The invention provides a lower limb skeleton rehabilitation robot motion control method based on interference compensation. Secondly, a force sensor measurement mode is adopted to estimate and compensate the load moment of the rehabilitation robot. And then, compensating the friction torque and the nominal quantity of the system by adopting a friction modeling and equivalent control mode. Finally, the unknown uncertainties of the hip joint system and the knee joint system are compensated online in real time in a disturbance observer mode, so that the comfort level of the whole rehabilitee in assisting motion control is greatly improved.
The following will further explain and explain a motion control method of a lower limb skeletal rehabilitation robot based on interference compensation according to the present invention with reference to the accompanying drawings. Referring to fig. 1, the method for controlling the motion of the lower limb skeletal rehabilitation robot based on interference compensation includes the following steps:
step S10, respectively installing FUTEK LSB200 type force sensors on a thigh rod and a shank rod of the lower limb skeletal rehabilitation robot, measuring load forces of a hip joint and a knee joint, and respectively estimating load moments of the hip joint and the knee joint according to the positions of the sensors;
specifically, first, a force sensor FUTEK LSB200 type is installed at a position C of the lower limb skeletal rehabilitation robot shown in fig. 2, and hip joint load force is measured and denoted as T 1 (ii) a The distance between hip joints A and C is measured and is denoted l a1 . Measuring the distance between the thigh rods A and D of the lower limb skeleton rehabilitation robot, and recording the distance as l 1
Next, a force sensor FUTEK LSB200 was installed at F position of the lower limb skeletal rehabilitation robot shown in FIG. 2, and knee joint load force was measured and recorded as T 2 (ii) a Measuring the distance between the knee joints D and F, denoted as l a2 . Measuring the distance between the shank rods D and G of the lower limb skeleton rehabilitation robot, and recording the distance as l 2
Finally, the hip and knee joint load moments are estimated as follows:
M f1 =l a1 T 1 ;M f2 =l a2 T 2
wherein M is f1 For hip load moment estimation, M f2 The estimated value of the hip joint load moment is obtained.
Step S20, respectively installing incremental orthogonal photoelectric encoders on a thigh rod and a shank rod of the lower limb skeletal rehabilitation robot, measuring a hip joint rotation angle and a knee joint rotation angle of the skeletal robot, obtaining a hip joint angle error and a knee joint angle error according to motion data of human gait, and respectively integrating to obtain error integral signals;
specifically, firstly, an incremental orthogonal photoelectric encoder is installed at the position B of the lower limb skeleton rehabilitation robot shown in fig. 2, and measurement is performedThe angle of rotation of the hip joint, denoted q 1 (ii) a An incremental orthogonal photoelectric encoder is arranged at the E position to measure the rotation angle of the knee joint and is recorded as q 2
Secondly, setting the expected values of the hip joint rotation angle and the knee joint rotation angle according to the motion data of human gait as follows:
q d1 =a 11 sin(b 11 p+c 11 )+a 12 sin(b 12 p+c 12 )+a 13 sin(b 13 p+c 13 );
q d2 =a 21 sin(b 21 p+c 21 )+a 22 sin(b 22 p+c 22 )+a 23 sin(b 23 p+c 23 );
wherein q is d1 As desired value of hip joint rotation angle, q d2 Is a desired value of the knee joint rotation angle, a 11 、a 12 、a 13 、a 21 、a 22 、a 23 、b 11 、b 12 、b 13 、b 21 、b 22 、b 23 、c 11 、c 12 、c 13 、c 21 、c 22 、c 23 The detailed design of the human body is shown in the embodiment of the later-written case, which is gait data. p is the percentage of the gait cycle and is calculated as follows:
p=t/T aa -floor(t/T aa );
wherein T is the exercise time of the rehabilitee, T aa The average period of the exercise steps of the rehabilitee. floor (T/T) indicates that integer parts are taken to the left, such as floor (3.5) =3, and finally the obtained p is more than or equal to 0 and less than or equal to 1.
Finally, comparing the measured value of the hip joint rotation angle with an expected value to obtain a hip joint rotation angle error signal, and recording the error signal as e 1 The calculation method is as follows:
e 1 =q 1 -q d1
comparing the measured value of the knee joint rotation angle with an expected value to obtain a knee joint rotation angle error signal marked as e 2 The calculation method is as follows:
e 2 =q 2 -q d2
integrating according to the hip joint rotation angle error signal to obtain a hip joint rotation angle error integral signal which is recorded as s 1 The calculation method is as follows:
s 1 =∫e 1 dt;
where dt represents the integration of the time signal.
Integrating according to the knee joint corner error signal to obtain a knee joint corner error integral signal, and recording as s 2 The calculation method is as follows:
s 2 =∫e 2 dt;
where dt represents integrating the time signal.
S30, constructing a nonlinear filtering differentiator according to the measurement signals of the hip joint rotation angle and the knee joint rotation angle to obtain a hip joint angular rate signal and a knee joint angular rate estimation signal;
specifically, first, the measurement signal q is obtained from the hip joint rotation angle 1 Establishing a nonlinear filter differentiator for calculating the hip joint rotation angular velocity estimation signal, denoted as D 1 The calculation method is as follows:
Figure GDA0003990282560000121
D 1 (n)=(q 1 (n)-q 1a (n))/(T a |q 1 (n)-q 1a (n)|+ε 1 );
wherein q is 1a (n) is a filtered hip joint rotation angle signal, T a 、ε 1 The detailed design of the parameter is described in the following examples. D 1 (n) is D 1 T is the time interval between data.
Secondly, measuring the signal q according to the knee joint rotation angle 2 Establishing a nonlinear filtering differentiator for calculating the knee joint rotation angular velocity estimation signal, and recording the signal as D 2 The calculation method is as follows:
Figure GDA0003990282560000122
D 2 (n)=(q 2 (n)-q 2a (n))/(T b |q 2 (n)-q 2a (n)|+ε 2 );
wherein q is 2a (n) is a filtered knee joint corner signal, T b 、ε 2 The detailed design of the parameter is described in the following examples. D 2 (n) is D 2 T is the time interval between data.
S40, measuring the weight of a thigh support rod and the weight of a shank support rod, constructing a skeleton system angular acceleration matrix and an angular velocity matrix according to the hip joint angle measurement value and the knee joint angle measurement value, and the length of the thigh support rod and the length of the shank support rod, and performing inverse transformation to obtain a skeleton system angular acceleration inverse matrix;
specifically, first, the weight of the thigh strut is measured, and is designated as m 1 Measuring the weight of the shank strut, and recording as m 2
Secondly, calculating the element values of the bone system angular acceleration matrix according to the hip joint angle measurement value and the knee joint angle measurement value as follows:
Figure GDA0003990282560000131
Figure GDA0003990282560000132
Figure GDA0003990282560000133
Figure GDA0003990282560000134
then, according to the element values of the bone system angular acceleration matrix, a bone system angular acceleration matrix is constructed, denoted as M, and composed as follows:
Figure GDA0003990282560000135
and solving the inverse matrix of the bone angular acceleration system, and recording the inverse matrix as M 0 Which satisfies M 0 M = E, where E is the identity matrix. At the same time M 0 The composition of (A) is as follows:
Figure GDA0003990282560000136
finally, according to the hip joint angle measurement value, the knee joint angle measurement value and the angular velocity estimation value thereof, calculating the element values of the bone system angular velocity matrix as follows:
Π 11 =-m 2 l 1 l 2 cos(q 2 )D 2
Π 12 =-m 2 l 1 l 2 cos(q 2 )D 2 /2;
Π 21 =-m 2 l 1 l 2 cos(q 2 )D 2 /2;
Π 22 =0;
constructing a bone system angular velocity matrix according to elements of the bone system angular velocity matrix, wherein the bone system angular velocity matrix is denoted as C and comprises the following components:
Figure GDA0003990282560000141
s50, calculating an angular velocity equivalent control quantity according to the hip joint rotation angular velocity estimation value and the knee joint rotation angular velocity estimation value, and calculating a thigh rod gravity related quantity and a shank rod gravity related quantity according to physical structure data of the rehabilitation robot;
specifically, firstly, the angular velocity of the hip joint is determined according to the rotation speedCalculating the angular velocity equivalent control quantity recorded as H by using the degree estimation value, the knee joint rotation angular velocity estimation value and the bone system angular velocity matrix 0a The composition mode is as follows:
Figure GDA0003990282560000142
wherein h is 0a1 Is the equivalent control quantity of the angular velocity of the hip joint, h 0a2 Is the knee joint angular velocity equivalent control quantity. H 0a The calculation method of (c) is as follows:
Figure GDA0003990282560000143
wherein D 1 Estimating a signal for said angular velocity of rotation of the hip joint, D 2 And estimating signals for the knee joint rotation angular velocity.
Secondly, according to the signals of the rotation angles of the hip joint and the knee joint and the physical structure data of the rehabilitation robot, the gravity related quantity of the thigh rod and the gravity related quantity of the shank rod are calculated and recorded as g 1 And g 2 The calculation method is as follows:
g 1 =-m 1 gl 1 sin(q 1 )/2-m 2 gl 2 sin(q 1 -q 2 )/2-m 2 gl 1 sin(q 1 );
g 2 =-m 2 gl 2 sin(q 1 -q 2 )/2;
wherein g is a gravity acceleration constant, and 9.8 is taken.
Step S60, compensating and designing the friction force of the hip joint and the friction force of the knee joint according to the estimated value of the rotation angle rate of the hip joint and the knee joint, and then calculating the equivalent control quantity of the hip joint and the knee joint according to the estimated value of the load moment of the hip joint and the knee joint and the inverse matrix of a bone angular acceleration system;
specifically, firstly, the estimated value D of the rotation angle rate of the hip joint and the knee joint is obtained 1 And D 2 Respectively calculating the friction compensation between hip joint and knee joint, and respectively recording the compensation as f a1 And f a2 The calculation method is as follows:
f a1 =f s λ 1 +f c (1-λ 1 );
f a2 =f s λ 2 +f c (1-λ 2 );
Figure GDA0003990282560000151
Figure GDA0003990282560000152
wherein f is s And f c Are estimated values of static friction coefficient and dynamic friction coefficient, respectively.
Secondly, according to the load moment estimated value M of the hip joint and the knee joint f1 And M f2 Angular velocity equivalent control quantity H 0a Hip joint and knee joint friction compensation amount f a1 And f a2 And the thigh bar weight-related quantity g and the shank bar weight-related quantity g 1 And g 2 Calculating the equivalent control quantity of hip joint and knee joint, respectively recording as u 1e And u 2e The calculation method is as follows:
Figure GDA0003990282560000153
step S70, constructing a hip joint and knee joint interference observer according to the equivalent control quantity of the hip joint and the knee joint, and respectively calculating the state and the interference estimation value of the hip joint and knee joint interference observer;
specifically, first, a hip joint disturbance observer is designed as follows:
Figure GDA0003990282560000154
Figure GDA0003990282560000155
Figure GDA0003990282560000156
Figure GDA0003990282560000157
x 12d =k a1 D 1 +k b1 q 1
Figure GDA0003990282560000161
wherein k is a1 、k b1 The detailed design of the parameter is described in the following examples.
Figure GDA0003990282560000162
For hip joint rotation angle desired value q d1 The derivative of (c). Wherein D 1d Is calculated as follows:
D 1d =u 1e -Y 110 u 1 -Y 120 u 2
wherein u is 1e For hip joint equivalent control quantity, u 1 Control of moment, u, for the hip joint 2 Controlling moment of knee joint, Y 110 、Y 120 For the bone angular acceleration system inverse matrix M 0 The elements of (a) are as follows:
Figure GDA0003990282560000163
to obtain finally
Figure GDA0003990282560000164
I.e. the hip joint disturbance estimate, w 1a Is the hip joint disturbance observer state.
Secondly, the knee joint disturbance observer is designed as follows:
Figure GDA0003990282560000165
Figure GDA0003990282560000166
Figure GDA0003990282560000167
Figure GDA0003990282560000168
x 22d =k a2 D 2 +k b2 q 2
Figure GDA0003990282560000169
wherein k is a2 、k b2 The detailed design of the parameter is described in the following examples.
Figure GDA00039902825600001610
For knee joint rotation angle desired value q d2 The derivative of (c). Wherein D 2d Is calculated as follows:
D 2d =u 2e -Y 210 u 1 -Y 220 u 2
wherein u is 2e For knee joint equivalent control quantity, u 1 Control of moment, u, for the hip joint 2 Controlling moment of knee joint, Y 210 、Y 220 For the bone angular acceleration system inverse matrix M 0 The elements of (a) are as follows:
Figure GDA00039902825600001611
to finally obtain
Figure GDA00039902825600001612
I.e. knee joint disturbance estimate, w 2a Is the knee joint disturbance observer state.
And S80, carrying out linear combination according to the hip joint and knee joint interference estimated value, the hip joint and knee joint rotation angle error amount, the error integral amount and the hip joint and knee joint rotation angle rate signal estimated value to generate hip joint and knee joint control moment, and realizing the final motion control of the lower limb skeleton rehabilitation robot.
Specifically, firstly, the estimated value of the hip joint interference is obtained
Figure GDA0003990282560000171
Error of hip joint rotation angle e 1 And an error integral quantity s 1 Hip joint equivalent control quantity u 1e Hip joint rotation angular rate signal estimation value D 1 Linear combination is carried out to generate hip joint control moment u 1 The calculation method is as follows:
Figure GDA0003990282560000172
wherein k is 11 、k 12 And k 13 The detailed design of the parameter is described in the following examples.
Secondly, estimating the value according to the knee joint interference
Figure GDA0003990282560000173
Error of knee joint rotation angle e 2 Error integral quantity s 2 Knee joint equivalent control quantity u 2e And knee joint rotation angle rate signal estimated value D 2 Linearly combined to generate knee joint control moment u 2 The calculation method is as follows:
Figure GDA0003990282560000174
wherein k is 21 、k 22 And k is 23 The detailed design of the parameter is described in the following examples.
And finally, the generated hip joint control moment and knee joint control moment are transmitted to a lower limb skeleton rehabilitation robot system, so that the rehabilitation person can be assisted to walk.
Case implementation and computer simulation result analysis
In order to verify the correctness and the effectiveness of the method provided by the invention, the following case simulation is provided for simulation.
In step S10, the measurement yields l 1 =0.52,l 2 =0.42. In step S20, T is selected aa =2,a 11 =20、a 12 =81、a 13 =2.5、a 21 =34、a 22 =8、a 23 =23、b 11 =7.3、b 12 =0.07、b 13 =17、b 21 =2、b 22 =15、b 23 =11、c 11 =1.1、c 12 =-0.02、c 13 =-4.7、c 21 =-3.4、c 22 =-1、c 23 And (4) = -2.3. The expected hip joint rotation angle values are shown in fig. 3, the measured hip joint rotation angles are shown in fig. 4, the comparison between the two values is shown in fig. 5, the hip joint rotation error angles are shown in fig. 6, the expected knee joint rotation angles are shown in fig. 7, the measured knee joint rotation angles are shown in fig. 8, and the comparison between the two values is shown in fig. 9. The knee joint rotation error angle is shown in fig. 10. In step S30, T is selected a =0.01、ε 1 =0.05、T b =0.01、ε 2 Fig. 11 shows hip joint rotational angular velocity estimation signals obtained by =0.05 and T =0.001, and fig. 12 shows knee joint rotational angular velocity estimation signals obtained by the same.
In step S40 and step S50, m is measured 1 =1.5,m 2 And =1. In step S60, f is selected s =0.05 and f c =0.05. In step S70, k is selected a1 =5、k b1 =0.3、k a2 =5、k b2 The estimated hip joint interference value was obtained as shown in fig. 13 and the estimated knee joint interference value was obtained as shown in fig. 14, = 0.3.
In step S80, k is selected 11 =255、k 12 =20、k 13 =2、k 21 =255、k 22 =20、k 23 =2, the final hip joint control moment is shown in fig. 15, and the knee joint control moment is shown in fig. 16.
As can be seen from fig. 13 and 14, the method of the present invention provides an estimation of unknown disturbance moments and system uncertainty in the hip and knee joints, the estimation fluctuating with gait. It can be seen from fig. 5 and 9 that the method provided by the present invention can better track the expected gait, while fig. 6 and 10 show the tracking error in the presence of system interference and uncertainty, and it can be seen that the error fluctuates around the value of 0, and the tracking effect can meet the needs of the rehabilitee as can be seen from the comparison curves shown in fig. 5 and 9. Fig. 11 and 12 show the estimated hip joint rotation angle rate and knee joint rotation angle rate. The invention only simply measures the rotation angles of the hip joint and the knee joint, but does not measure the angular rates of the hip joint and the knee joint, and carries out approximate estimation solution of digital filter differentiation. Therefore, the estimated value can completely meet the control requirement of the rehabilitation robot, so that the installation of an angular velocity measurement component is saved, and the whole method is simple and economical. In conclusion, the lower limb skeleton rehabilitation robot method provided by the invention is completely feasible and has high engineering practical value.
Other embodiments of the invention will be apparent to those skilled in the art from consideration of the specification and practice of the invention disclosed herein. This application is intended to cover any variations, uses, or adaptations of the invention following, in general, the principles of the invention and including such departures from the present disclosure as come within known or customary practice within the art to which the invention pertains. It is intended that the specification and examples be considered as exemplary only, with a true scope and spirit of the invention being indicated by the following claims.

Claims (1)

1. A lower limb skeleton rehabilitation robot based on interference compensation is characterized in that an FUTEK LSB200 type force sensor is installed at a selected position on a thigh between a hip joint A and a knee joint D of the lower limb skeleton rehabilitation robot, the position is marked as C, hip joint load force is measured, and the position is marked as T 1 (ii) a The distance between hip joints A and C is measured and is denoted as l a1 (ii) a Measuring the distance between the hip joint A and the knee joint D of the lower limb skeleton rehabilitation robot thigh rod, and recording as l 1 (ii) a Secondly, an FUTEKLSB200 type force sensor is arranged at a selected position on the lower leg of the lower limb bone rehabilitation robot, the installation position is marked as F, the knee joint load force is measured and is marked as T 2 (ii) a The distance between the knee joints D and F is measured and recorded as l a2 (ii) a Measuring the distance from knee joint D to heel G of lower limb skeleton rehabilitation robot calf rod, and recording as l 2 (ii) a The hip and knee joint load moments are estimated as follows:
M f1 =l a1 T 1 ;M f2 =l a2 T 2
wherein M is f1 For hip load moment estimation, M f2 The estimated value of hip joint load moment is obtained; installing an incremental orthogonal photoelectric encoder at a position B between a hip joint A and a knee joint D of the lower limb skeleton rehabilitation robot, measuring the rotation angle of the hip joint and recording the rotation angle as q 1 (ii) a An incremental orthogonal photoelectric encoder is arranged at a position selected between the D position and the F position of the knee joint, the position is marked as E, the rotation angle of the knee joint is measured, and the position is marked as q 2 Obtaining hip joint angle errors and knee joint angle errors according to the motion data of human gait, and respectively integrating to obtain error integral signals; constructing a nonlinear filtering differentiator according to the measurement signals of the hip joint rotation angle and the knee joint rotation angle to obtain a hip joint angular velocity signal and a knee joint angular velocity estimation signal; then measuring the weight of the thigh support rod and the weight of the shank support rod, and constructing the angular acceleration moment of the skeletal system according to the hip joint rotation angle measurement value, the knee joint rotation angle measurement value, the length of the thigh support rod and the length of the shank support rodPerforming inverse transformation on the matrix and the angular velocity matrix to obtain an angular acceleration inverse matrix of the bone system; calculating an angular velocity equivalent control quantity according to the hip joint rotation angular velocity estimation value and the knee joint rotation angular velocity estimation value, and calculating a thigh rod gravity related quantity and a shank rod gravity related quantity according to physical structure data of the rehabilitation robot; then according to the estimated value of the rotation angular velocity of the hip joint and the knee joint, carrying out compensation design on the friction force of the hip joint and the friction force of the knee joint, and then according to the estimated value of the load moment of the hip joint and the knee joint and an inverse matrix of a bone angular acceleration system, calculating the equivalent control quantity of the hip joint and the knee joint; then constructing a hip joint and knee joint interference observer according to the equivalent control quantity of the hip joint and the knee joint, and respectively calculating the state and the interference estimation value of the hip joint and knee joint interference observer; finally, carrying out linear combination according to the hip joint and knee joint interference estimation value, the hip joint and knee joint rotation angle error amount, the error integral amount and the hip joint and knee joint rotation angular velocity signal estimation value to generate hip joint and knee joint control torque so as to realize final motion control of the lower limb skeleton rehabilitation robot; the specific resolving method of the related signals comprises the following seven parts:
the first part is to perform integration according to the hip joint angle error and the knee joint angle error respectively to obtain an error integration signal, and the operation is as follows:
q d1 =a 11 sin(b 11 p+c 11 )+a 12 sin(b 12 p+c 12 )+a 13 sin(b 13 p+c 13 );
q d2 =a 21 sin(b 21 p+c 21 )+a 22 sin(b 22 p+c 22 )+a 23 sin(b 23 p+c 23 );
p=t/T aa -floor(t/T aa );
e 1 =q 1 -q d1
e 2 =q 2 -q d2
s 1 =∫e 1 dt;
s 2 =∫e 2 dt;
wherein q is 1 Measurement of hip joint rotation angle for a lower extremity skeletal rehabilitation robot, q 2 Measuring the knee joint rotation angle of the lower limb skeleton rehabilitation robot; q. q.s d1 As desired value of hip joint rotation angle, q d2 Is a desired value of the knee joint rotation angle, a 11 、a 12 、a 13 、a 21 、a 22 、a 23 、b 11 、b 12 、b 13 、b 21 、b 22 、b 23 、c 11 、c 12 、c 13 、c 21 、c 22 、c 23 Human body gait data; p is the percentage of the gait cycle, T is the locomotion time of the rehabilitee, T aa Average period of exercise steps for the rehabilitee; floor (T/T) represents that an integral part is taken to the left, if floor (3.5) =3, p is more than or equal to 0 and less than or equal to 1 finally obtained; e.g. of the type 1 For hip joint angle error signals, e 2 Is knee joint angle error signal, s 1 Integrating the hip joint rotation angle error signal, wherein dt represents the integration of the time signal; s 2 Integrating knee joint rotation angle error signals;
and a second part, constructing a nonlinear filtering differentiator according to the measurement signals of the hip joint rotation angle and the knee joint rotation angle, and solving to obtain a hip joint angular velocity signal and a knee joint angular velocity estimation signal as follows:
Figure FDA0003990282550000031
D 1 (n)=(q 1 (n)-q 1a (n))/(T a |q 1 (n)-q 1a (n)|+ε 1 );
Figure FDA0003990282550000032
D 2 (n)=(q 2 (n)-q 2a (n))/(T b |q 2 (n)-q 2a (n)|+ε 2 );
wherein q is 1 For measuring signals of angle of rotation of the hip joint, D 1 Estimating a signal for the angular velocity of rotation of the hip joint, q 1a (n) is a filtered hip joint rotation angle signal, T a 、ε 1 、T b 、ε 2 Is a constant parameter; q. q.s 2 For measuring knee joint rotation angle, D 2 Estimating a signal for knee joint rotational angular velocity, q 2a (n) is a filtered knee joint corner signal, and T is a time interval between data;
and a third part, measuring the weight of the thigh support rod and the weight of the crus support rod, constructing a skeleton system angular acceleration matrix and an angular velocity matrix according to the hip joint rotation angle measurement value and the knee joint rotation angle measurement value, and the length of the thigh support rod and the length of the crus support rod, and performing inverse transformation to obtain a skeleton system angular acceleration inverse matrix resolving formula as follows:
Figure FDA0003990282550000033
Figure FDA0003990282550000034
Figure FDA0003990282550000035
Figure FDA0003990282550000036
Figure FDA0003990282550000037
M 0 M=E;
Figure FDA0003990282550000038
Π 11 =-m 2 l 1 l 2 cos(q 2 )D 2
Π 12 =-m 2 l 1 l 2 cos(q 2 )D 2 /2;
Π 21 =-m 2 l 1 l 2 cos(q 2 )D 2 /2;
Π 22 =0;
Figure FDA0003990282550000041
wherein m is 1 The weight of the thigh strut, m 2 Is the weight of the leg strut, M is the skeletal system angular acceleration matrix, M 0 Is an inverse matrix of the angular acceleration of the skeletal system, and E is a unit matrix; c is a bone system angular velocity matrix;
fourthly, calculating equivalent angular velocity control quantity according to the hip joint rotation angular velocity estimation value and the knee joint rotation angular velocity estimation value, and then calculating the thigh rod gravity related quantity and the shank rod gravity related quantity according to physical structure data of the rehabilitation robot as follows:
Figure FDA0003990282550000042
g 1 =-m 1 gl 1 sin(q 1 )/2-m 2 gl 2 sin(q 1 -q 2 )/2-m 2 gl 1 sin(q 1 );
g 2 =-m 2 gl 2 sin(q 1 -q 2 )/2;
wherein H 0a For angular velocity equivalent control quantity, D 1 Estimating a signal for said angular velocity of rotation of the hip joint, D 2 Estimating a signal for said knee joint rotational angular velocity, h 0a1 Is the angular velocity equivalent control quantity of the hip joint, h 0a2 Is the knee joint angular velocity equivalent control quantity, g 1 And g 2 G is a gravity acceleration constant, and the value of g is 9.8;
and a fifth step of performing compensation design on the friction force of the hip joint and the friction force of the knee joint according to the estimated value of the rotation angular velocity of the hip joint and the knee joint, and then performing a calculation method on the equivalent control quantity of the hip joint and the knee joint according to the estimated value of the load moment of the hip joint and the knee joint and an inverse matrix of a bone angular acceleration system as follows:
f a1 =f s λ 1 +f c (1-λ 1 );
f a2 =f s λ 2 +f c (1-λ 2 );
Figure FDA0003990282550000043
Figure FDA0003990282550000051
M f1 =l a1 T 1
M f2 =l a2 T 2
Figure FDA0003990282550000052
wherein T is 1 And T 2 Measured values of the load forces of the hip and knee joints, respectively,/ a1 Is the distance between the hip joint and the installation position of the hip load force sensor, l a2 The distance between the knee joint and the installation position of the knee load force sensor; d 1 And D 2 Is an estimate of the angular velocity of rotation of the hip and knee joints, f a1 And f a2 The compensation amount of the friction force between the hip joint and the knee joint, f s And f c Is divided intoThe estimated values of the static friction coefficient and the dynamic friction coefficient are respectively; m f1 And M f2 For hip and knee load moment estimates, H 0a For the equivalent control quantity of angular velocity, g 1 And g 2 The thigh bar gravity related quantity and the shank bar gravity related quantity u 1e And u 2e Calculating the equivalent control quantity of the hip joint and the knee joint finally;
and a sixth part, constructing a hip joint and knee joint interference observer according to the equivalent control quantity of the hip joint and the knee joint, wherein the calculation mode of the state and the interference estimation value of the hip joint and knee joint interference observer is as follows:
Figure FDA0003990282550000053
Figure FDA0003990282550000054
Figure FDA0003990282550000055
Figure FDA0003990282550000056
x 12d =k a1 D 1 +k b1 q 1
Figure FDA0003990282550000057
D 1d =u 1e -Y 110 u 1 -Y 120 u 2
Figure FDA0003990282550000058
Figure FDA0003990282550000059
Figure FDA0003990282550000061
Figure FDA0003990282550000062
x 22d =k a2 D 2 +k b2 q 2
Figure FDA0003990282550000063
D 2d =u 2e -Y 210 u 1 -Y 220 u 2
Figure FDA0003990282550000064
wherein k is a1 、k b1 、k a2 、k b2 Is a constant parameter;
Figure FDA0003990282550000065
for hip joint rotation angle desired value q d1 A derivative of (a);
Figure FDA0003990282550000066
for knee joint rotation angle desired value q d2 A derivative of (a); u. of 1e For hip joint equivalent control quantity, u 1 Control of moment, u, for the hip joint 2e For knee joint equivalent control quantity, u 2 Controlling moment of knee joint, Y 110 、Y 120 、Y 210 、Y 220 For the bone angular acceleration system inverse matrix M 0 To finally obtain
Figure FDA0003990282550000067
As an estimate of hip joint interference, w 1a Is the state of the hip joint disturbance observer;
Figure FDA0003990282550000068
as knee joint disturbance estimate, w 2a Is the knee joint disturbance observer state;
and the seventh part is that the method for generating the control moment of the hip joint and the knee joint is performed by linear combination according to the estimated value of the interference between the hip joint and the knee joint, the error amount of the rotation angle of the hip joint and the knee joint, the error integral amount and the estimated value of the rotation angular velocity signal of the hip joint and the knee joint, and comprises the following steps:
Figure FDA0003990282550000069
Figure FDA00039902825500000610
wherein
Figure FDA00039902825500000611
And
Figure FDA00039902825500000612
respectively as said hip and knee joint interference estimates, e 1 And e 2 The difference between the rotation angle of hip joint and knee joint, s 1 And s 2 Are respectively the rotating angle error integral quantity u of the hip joint and the knee joint 1e And u 2e Respectively, the equivalent control quantity of hip joint and knee joint, D 1 And D 2 The angular velocity signal estimates u for hip joint and knee joint, respectively 1 And u 2 For the resulting hip and knee control moment, k 21 、k 22 And k is 23 、k 11 、k 12 And k 13 Is a constant parameter; the generated hip joint control moment and knee joint control moment are transmitted to a lower limb skeleton rehabilitation robot system, and the motion and control of the robot can be realized.
CN202010404277.6A 2020-05-13 2020-05-13 Lower limb skeleton rehabilitation robot motion control method based on interference compensation Active CN112089580B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010404277.6A CN112089580B (en) 2020-05-13 2020-05-13 Lower limb skeleton rehabilitation robot motion control method based on interference compensation

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010404277.6A CN112089580B (en) 2020-05-13 2020-05-13 Lower limb skeleton rehabilitation robot motion control method based on interference compensation

Publications (2)

Publication Number Publication Date
CN112089580A CN112089580A (en) 2020-12-18
CN112089580B true CN112089580B (en) 2023-02-28

Family

ID=73750083

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010404277.6A Active CN112089580B (en) 2020-05-13 2020-05-13 Lower limb skeleton rehabilitation robot motion control method based on interference compensation

Country Status (1)

Country Link
CN (1) CN112089580B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113855472B (en) * 2021-08-25 2023-11-28 上海傅利叶智能科技有限公司 Method and device for controlling exoskeleton robot and exoskeleton robot
CN113855473B (en) * 2021-08-25 2023-08-15 上海傅利叶智能科技有限公司 Method and device for controlling exoskeleton robot and exoskeleton robot

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101204347A (en) * 2007-12-06 2008-06-25 上海大学 Automatic gait correcting device in lower limb rehabilitation
CN103212188A (en) * 2013-05-13 2013-07-24 中山大学 Method and system for assisting gait training
WO2016088842A1 (en) * 2014-12-03 2016-06-09 国立大学法人北海道大学 Gait analysis method and gait analysis system
CN207886415U (en) * 2017-08-17 2018-09-21 四川汇智众创科技有限公司 A kind of wearable exoskeleton lower limb rehabilitation robot
CN110575362A (en) * 2019-09-25 2019-12-17 深圳市丞辉威世智能科技有限公司 Limb rehabilitation exoskeleton and limb rehabilitation system

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106983589B (en) * 2017-04-07 2018-07-31 河北工业大学 A kind of active above-knee prosthesis TSM control method based on interference observer
CN110215648B (en) * 2019-06-28 2021-03-26 华中科技大学 Exoskeleton coordination gait control method based on human body gait motion coordination characteristic

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101204347A (en) * 2007-12-06 2008-06-25 上海大学 Automatic gait correcting device in lower limb rehabilitation
CN103212188A (en) * 2013-05-13 2013-07-24 中山大学 Method and system for assisting gait training
WO2016088842A1 (en) * 2014-12-03 2016-06-09 国立大学法人北海道大学 Gait analysis method and gait analysis system
CN207886415U (en) * 2017-08-17 2018-09-21 四川汇智众创科技有限公司 A kind of wearable exoskeleton lower limb rehabilitation robot
CN110575362A (en) * 2019-09-25 2019-12-17 深圳市丞辉威世智能科技有限公司 Limb rehabilitation exoskeleton and limb rehabilitation system

Also Published As

Publication number Publication date
CN112089580A (en) 2020-12-18

Similar Documents

Publication Publication Date Title
CN104757976B (en) A kind of Human Body Gait Analysis method and system based on Multi-sensor Fusion
CN112089580B (en) Lower limb skeleton rehabilitation robot motion control method based on interference compensation
Schneider et al. Mass, center of mass, and moment of inertia estimates for infant limb segments
Rouse et al. Development of a mechatronic platform and validation of methods for estimating ankle stiffness during the stance phase of walking
Li et al. Walking speed and slope estimation using shank-mounted inertial measurement units
US8315823B2 (en) Force and/or motion measurement system having inertial compensation and method thereof
Li et al. The lower limbs kinematics analysis by wearable sensor shoes
CN108836346A (en) A kind of Human Body Gait Analysis method and system based on inertial sensor
Bergmann et al. Lower limb exoskeleton with compliant actuators: Design, modeling, and human torque estimation
Ladin et al. Combining position and acceleration measurements for joint force estimation
Li et al. Reconstructing walking dynamics from two shank-mounted inertial measurement units
Lauder et al. Estimating propulsive forces—sink or swim?
KR20200072867A (en) Method for controlling a walking assist device and electronic device performing the method
Gorelick et al. Test–retest reliability of the IDEEA system in the quantification of step parameters during walking and stair climbing
Farjadian et al. VRACK: Measuring pedal kinematics during stationary bike cycling
Adiputra et al. Improving passive ankle foot orthosis system using estimated ankle velocity reference
Bishop et al. Walking speed estimation using shank-mounted accelerometers
Xu et al. A novel motion platform system for testing prosthetic knees
Allen et al. A nonlinear dynamics-based estimator for functional electrical stimulation: Preliminary results from lower-leg extension experiments
CN115607146B (en) Wearable single-node device for leg posture estimation and measurement method
CN116115217B (en) Human lower limb gait phase estimation method based on depth network
Yi et al. Sensor-movement-robust angle estimation for 3-DoF lower limb joints without calibration
Caporaso et al. Digital human models for gait analysis: experimental validation of static force analysis tools under dynamic conditions
Lee et al. Estimation of unmeasured golf swing of arm based on the swing dynamics
Zhong Development And Experimental Evaluation Of A State Dependent Coefficient Based State Estimator For Functional Electrical Stimulation-elicited Tasks

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