CN112089580B - A Motion Control Method for Lower Limb Skeletal Rehabilitation Robot Based on Disturbance Compensation - Google Patents

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

A Motion Control Method for Lower Limb Skeletal Rehabilitation Robot Based on Disturbance Compensation

technical field

The invention relates to the field of motion control of a lower limb bone rehabilitation robot, in particular to a motion control method for a lower limb bone rehabilitation robot based on interference compensation.

Background technique

With the development of society, my country's population structure has a clear trend of aging. A large number of elderly people have cardiovascular and cerebrovascular diseases such as hemiplegia, while most stroke or other patients have different degrees of lower limb movement disorders. With the development of rehabilitation robot technology, rehabilitation robots are widely used in the early exercise rehabilitation treatment of the above-mentioned cases to assist the patient's movement. It can provide convenience for patients to take care of themselves in basic life care, reduce the burden of care, and improve the quality of life of the recovered patients. The difficulties in the motion control of the lower limb bone rehabilitation robot lie in the difficulty in accurately measuring the motion load in the actual process, the inaccurate estimation of the friction torque, and the unknown and uncertain external disturbances in the motion control. In the case of the above-mentioned strong uncertainties, ensuring the stability and safety of rehabilitation motion control and improving its comfort are the main tasks of motion control. Based on the above reasons, the present invention proposes a control scheme for disturbance observation compensation for the three kinds of uncertainties, and adopts the method of error feedback and equivalent control to realize the high-quality control of the rehabilitation robot, which also makes the present invention have a good economic and practical value.

It should be noted that the information disclosed in the above background technology section is only used to enhance 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.

Contents of the invention

The purpose of the present invention is to provide a motion control method for a lower limb bone rehabilitation robot based on interference compensation, and then at least to a certain extent overcome the limitations and defects of related technologies caused by insufficient self-adaptive ability of anti-interference and load uncertainty. Problem with poor comfort of motion controls.

According to one aspect of the present invention, there is provided a motion control method for a lower limb bone rehabilitation robot based on interference compensation, comprising the following steps:

Step S10, install FUTEK LSB200 force sensors on the thigh bar and calf bar of the lower limb bone rehabilitation robot respectively, measure the load force of the hip joint and knee joint, and estimate the load moment of the hip joint and knee joint respectively according to the position of the sensor;

Step S20, install incremental orthogonal photoelectric encoders on the thigh bar and calf bar of the lower limb bone rehabilitation robot respectively, measure the hip joint rotation angle and knee joint rotation angle of the bone robot, and obtain the hip joint rotation angle according to the motion data of the human body gait. The joint angle error and the knee joint angle error are integrated separately to obtain an error integral signal;

Step S30, constructing a nonlinear filter differentiator according to the measurement signals of the hip joint rotation angle and the knee joint rotation angle to obtain the hip joint angular rate signal and the knee joint angular rate estimation signal;

Step S40, measuring the weight of the thigh strut and the weight of the calf strut, and constructing the angular acceleration of the skeletal system according to the measured values of the hip joint angle and the knee joint angle, as well as the lengths of the thigh strut and the calf strut Matrix and angular velocity matrix, and perform inverse transformation to obtain the inverse matrix of angular acceleration of the skeletal system;

Step S50, calculate the equivalent control quantity of angular velocity according to the estimated value of the hip joint rotation angular velocity and the knee joint rotation angular velocity, and then calculate the gravity related quantity of the thigh bar and the gravity related quantity of the calf bar according to the physical structure data of the rehabilitation robot ;

Step S60, according to the estimated value of hip joint and knee joint rotation angular rate, carry out compensation design for hip joint friction force and knee joint friction force, and then according to the described value of hip joint and knee joint load moment estimate and bone angular acceleration System inverse matrix, solve the equivalent control quantity of hip joint and knee joint;

Step S70, according to the equivalent control quantities of the hip joint and the knee joint, construct the interference observer of the hip joint and the knee joint, respectively solve the states and estimated values of the interference of the hip joint and the knee joint;

Step S80, according to the estimated hip joint and knee joint interference value, the hip joint and knee joint rotation angle error amount, the error integral amount and the hip joint and knee joint rotation angular rate signal estimated value, perform linear combination to generate the hip joint and knee joint Knee joint control moment for ultimate lower limb skeletal rehabilitation robot motion control.

In an exemplary embodiment of the present invention, the hip joint rotation angle and the knee joint rotation angle of the skeletal robot are measured, and according to the motion data of the human body gait, the hip joint angle error and the knee joint angle error are obtained and integrated respectively, The obtained error integral signal includes:

q _d1 =a ₁₁ sin(b ₁₁ p+c ₁₁ )+a ₁₂ sin(b ₁₂ p+c ₁₂ )+a ₁₃ sin(b ₁₃ p+c ₁₃ );

q _d2 =a ₂₁ sin(b ₂₁ p+c ₂₁ )+a ₂₂ sin(b ₂₂ p+c ₂₂ )+a ₂₃ sin(b ₂₃ p+c ₂₃ );

p=t/T _aa -floor(t/T _aa );

e ₁ =q ₁ -q _d1 ;

e ₂ =q ₂ -q _d2 ;

s ₁ =∫ e ₁ dt;

s ₂ =∫e ₂ dt;

Among them, q ₁ is the measured value of the hip joint rotation angle of the lower limb skeletal rehabilitation robot, and q ₂ is the measured value of the knee joint rotation angle of the lower limb skeletal rehabilitation robot. q _d1 is the expected value of hip joint rotation angle, q _d2 is the expected value of knee joint rotation _angle , a ₁₁ , a ₁₂ , a ₁₃ , a 21 , a ₂₂ , a ₂₃ , b ₁₁ , b ₁₂ , b ₁₃ , b ₂₁ , b ₂₂ , b ₂₃ , c ₁₁ , c ₁₂ , c ₁₃ , c ₂₁ , c ₂₂ , and c ₂₃ are the gait data of the human body, and its detailed design is shown in the following case implementation. p is the percentage of gait cycle, t is the exercise time of the rehabilitated person, and T _aa is the average cycle of the rehabilitated person's exercise pace. floor(t/T) represents taking the integer part to the left, such as floor(3.5)=3, and the final result is 0≤p≤1. e ₁ is the hip joint rotation angle error signal, e ₂ is the knee joint rotation angle error signal, s ₁ is the hip joint rotation angle error integral signal, where dt represents the integration of the time signal. s ₂ is the integral signal of knee joint rotation angle error.

In an exemplary embodiment of the present invention, according to the measurement signals of the hip joint rotation angle and the knee joint rotation angle, a nonlinear filter differentiator is constructed to obtain the hip joint angular rate signal and the knee joint angular rate estimation signal including:

D ₁ (n)=(q ₁ (n)-q _1a (n))/(T _a |q ₁ (n)-q _1a (n)|+ε ₁ );

D ₂ (n)=(q ₂ (n)-q _2a (n))/(T _b |q ₂ (n)-q _2a (n)|+ε ₂ );

where q ₁ is the measurement signal of hip joint rotation angle, D ₁ is the estimated signal of hip joint rotation angle velocity, q _1a (n) is the filtered hip joint rotation angle signal, T _a , ε ₁ , T _b , ε ₂ are constant parameters, and For detailed design, see the following case implementation. q ₂ is the measurement signal of knee joint rotation angle, D ₂ is the estimated signal of knee joint rotation angle velocity, q _2a (n) is the filtered knee joint rotation angle signal, and T is the time interval between data.

In an exemplary embodiment of the present invention, according to the hip joint angle measurement value and the knee joint angle measurement value, as well as the length of the thigh strut and the length of the calf strut, the angular acceleration matrix and the angular velocity matrix of the skeletal system are constructed and Its inverse matrix includes:

M ₀ M=E;

Π ₁₁ ＝-m ₂ l ₁ l ₂ cos(q ₂ )D ₂ ;

Π ₁₂ ＝-m ₂ l ₁ l ₂ cos(q ₂ )D ₂ /2;

Π ₂₁ ＝-m ₂ l ₁ l ₂ cos(q ₂ )D ₂ /2;

Π ₂₂ =0;

Where m ₁ is the weight of the thigh strut, m ₂ is the weight of the calf strut, M is the angular acceleration matrix of the skeletal system, M ₀ is the inverse matrix of the angular acceleration of the skeletal system, and E is the identity matrix. C is the angular velocity matrix of the skeletal system.

In an exemplary embodiment of the present invention, the angular velocity equivalent control quantity is calculated according to the estimated hip joint rotational angular velocity and the knee joint rotational angular velocity, and then the gravity of the thigh bar is calculated according to the physical structure data of the rehabilitation robot Related quantities related to calf bar gravity include:

g ₁ =-m ₁ gl ₁ sin(q ₁ )/2-m ₂ gl ₂ sin(q ₁ -q ₂ )/2-m ₂ gl ₁ sin(q ₁ );

g ₂ =-m ₂ gl ₂ sin(q ₁ -q ₂ )/2;

Where H _0a is the equivalent control quantity of angular velocity, D ₁ is the estimated signal of hip joint rotational angular velocity, D ₂ is the described signal of knee joint rotational angular velocity estimation, h _0a1 is the equivalent control quantity of hip joint angular velocity, h _0a2 is The equivalent control quantity of the angular velocity of the knee joint, g ₁ and g ₂ are the gravity-related quantities of the thigh bar and the calf bar gravity-related quantities, g is the gravitational acceleration constant, and the value is 9.8.

In an example embodiment of the present invention, according to the estimated values of the rotational angular rates of the hip joint and the knee joint, compensation design is carried out for the friction force of the hip joint and the friction force of the knee joint, and the equivalent equation of the hip joint and the knee joint is calculated. Controls include:

f _a1 =f _s λ ₁ +f _c (1-λ ₁ );

f _a2 =f _s λ ₂ +f _c (1-λ ₂ );

M _f1 = l _a1 T ₁ ;

M _f2 = l _a2 T ₂ ;

Among them, T ₁ and T ₂ are the measured values of the load force of the hip joint and the knee joint respectively, l _a1 is the distance between the hip joint and the installation position of the hip load force sensor, and l _a2 is the distance between the knee joint and the installation position of the knee load force sensor distance. D ₁ and D ₂ are the estimated rotational angular rates of the hip and knee joints, f _a1 and f _a2 are the friction compensation amounts of the hip and knee joints, f _s and f _c are the estimates of the coefficient of static friction and the coefficient of dynamic friction, respectively value. M _f1 and M _f2 are the estimated load moments of the hip joint and knee joint, H _0a is the equivalent control value of the angular velocity, g ₁ and g ₂ are the gravity-related quantities of the thigh bar and the calf bar gravity-related quantities, and u _1e and u _2e are the final The calculated equivalent control amount of hip joint and knee joint.

In an exemplary embodiment of the present invention, according to the equivalent control quantities of the hip joint and the knee joint, a hip joint and knee joint interference observer is constructed, and the state and interference estimation of the hip joint and knee joint interference observer are solved respectively Values include:

x _12d =k _a1 D ₁ +k _b1 q ₁ ;

D _1d =u _1e -Y ₁₁₀ u ₁ -Y ₁₂₀ u ₂ ;

x _22d =k _a2 D ₂ +k _b2 q ₂ ;

D _2d =u _2e -Y ₂₁₀ u ₁ -Y ₂₂₀ u ₂ ;

为髋关节转动角期望值q_d1的导数。

为膝关节转动角期望值q_d2的导数。u_1e为髋关节等效控制量，u₁为髋关节控制力矩，u_2e为膝关节等效控制量，u₂为膝关节控制力矩，Y₁₁₀、Y₁₂₀、Y₂₁₀、Y₂₂₀为骨骼角加速度系统逆矩阵M₀的元素，最终得到

为髋关节干扰估计值，w_1a为髋关节干扰观测器状态。

为膝关节干扰估计值，w_2a为膝关节干扰观测器状态。Among them, k _a1 , k _b1 , k _a2 , and k _b2 are constant value parameters, and their detailed design is shown in the following case implementation.

is the derivative of the expected value q _d1 of the hip joint rotation angle.

is the derivative of the expected value q _d2 of the knee joint rotation angle. u _1e is the equivalent control quantity of the hip joint, u ₁ is the control torque of the hip joint, u _2e is the equivalent control quantity of the knee joint, u ₂ is the control torque of the knee joint, Y ₁₁₀ , Y ₁₂₀ , Y ₂₁₀ , and Y ₂₂₀ are bone angles The elements of the inverse matrix M ₀ of the acceleration system, and finally get

is the estimated value of hip joint interference, w _1a is the state of hip joint interference observer.

is the estimated value of the knee joint disturbance, w _2a is the state of the knee joint disturbance observer.

In an exemplary embodiment of the present invention, 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 integration amount, and the estimated value of the rotational angle rate signal of the hip joint and the knee joint, Perform linear combination to generate hip joint and knee joint control torque including:

与

分别为所述的髋关节与膝关节干扰估计值，e₁与e₂分别为髋关节与膝关节转动角误差量，s₁与s₂分别为髋关节与膝关节转动角误差积分量，u_1e与u_2e分别为髋关节与膝关节等效控制量，D₁与D₂分别为髋关节与膝关节转动角速率信号估计值，u₁与u₂为最终所得的髋关节与膝关节控制力矩，k₂₁、k₂₂与k₂₃、k₁₁、k₁₂与k₁₃为常值参数，其详细设计见后文案例实施。in

and

are the estimated values of interference between the hip joint and the knee joint, respectively, e ₁ and e ₂ are the rotational angle errors of the hip joint and the knee joint respectively, s ₁ and s ₂ are the integral rotational angle errors of the hip joint and the knee joint respectively, u _1e and u _2e are the equivalent control quantities of the hip joint and knee joint, respectively, D ₁ and D ₂ are the estimated values of the rotational angular rate signals of the hip joint and knee joint, respectively, and u ₁ and u ₂ are the final control of the hip joint and knee joint Torque, k ₂₁ , k ₂₂ and k ₂₃ , k ₁₁ , k ₁₂ and k ₁₃ are constant value parameters, the detailed design of which is shown in the following case implementation.

The generated hip joint control torque and knee joint control torque are sent to the lower limb bone rehabilitation robot system to assist the rehabilitation person to walk.

Beneficial effect

The present invention provides a motion control method for lower limb bone rehabilitation robot based on disturbance compensation, which has the advantage of adopting the method of disturbance observer at first, so that the load uncertainty, friction uncertainty and unknown disturbance in the rehabilitation robot are invariable. Certainty can be effectively compensated by estimation. Secondly, by using the method of equivalent uncertainty, the nominal quantity of the rehabilitation robot is equivalently estimated, and the load is measured and compensated, and the unknown friction torque is estimated and compensated. The compensation in the above two aspects can greatly improve the dynamic effect of the rehabilitation robot's gait tracking control, and improve the comfort of the rehabilitation person's assisted walking movement. Finally, the angular velocity of the hip and knee motion of the rehabilitation robot is estimated by means of nonlinear filter differentiation, which avoids the installation of angular velocity measurement components and reduces the economic cost of the rehabilitation robot. Therefore, the present invention has the advantages of economy, practicality and comfort, and has high engineering application and popularization value.

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.

Description of 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. Apparently, the drawings in the following description are only some embodiments of the present invention, and those skilled in the art can obtain other drawings according to these drawings without creative efforts.

Fig. 1 is a flow chart of a motion control method for a lower limb bone rehabilitation robot based on interference compensation provided by the present invention;

Fig. 2 is a schematic diagram of the geometric structure of the lower limb bone rehabilitation robot according to the method provided by the embodiment of the present invention;

Fig. 3 is the hip joint rotation angle expected value curve (unit: degree) of the method provided by the embodiment of the present invention;

Fig. 4 is the measured value curve (unit: degree) of the hip joint rotation angle of the method provided by the embodiment of the present invention;

Fig. 5 is a comparison curve (unit: degree) between the measured value of the hip joint rotation angle and the expected value according to the method provided by the embodiment of the present invention;

Fig. 6 is the hip joint rotation error angle curve (unit: degree) of the method provided by the embodiment of the present invention;

Fig. 7 is the expected value curve of knee joint rotation angle (unit: degree) according to the method provided by the embodiment of the present invention;

Fig. 8 is the knee joint rotation angle measured value curve (unit: degree) according to the method provided by the embodiment of the present invention;

Fig. 9 is a comparison curve (unit: degree) between the measured value of the knee joint rotation angle and the expected value according to the method provided by the embodiment of the present invention;

Fig. 10 is the knee joint rotation error angle curve (unit: degree) of the method provided by the embodiment of the present invention;

Fig. 11 is the hip joint rotation angular velocity estimation signal (unit: degree/second) of the method provided by the embodiment of the present invention;

Fig. 12 is the knee joint rotation angular velocity estimation signal (unit: degree/second) of the method provided by the embodiment of the present invention;

Fig. 13 is the hip joint interference estimated value (unit: Nm) of the method provided by the embodiment of the present invention;

Fig. 14 is the knee joint interference estimated value (unit: Nm) of the method provided by the embodiment of the present invention;

Fig. 15 is the hip joint control moment (unit: Nm) of the method provided by the embodiment of the present invention;

Fig. 16 is the knee joint control torque (unit: Nm) of the method provided by the embodiment of the present invention.

Detailed ways

Example embodiments will now be described more fully with reference to the accompanying drawings. Example embodiments may, however, be embodied in many 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 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 in order to give a thorough understanding of embodiments of the invention. However, those skilled in the art will appreciate that the technical solution of the present invention may be practiced without one or more of the specific details, or other methods, components, devices, steps, etc. may be adopted. In other instances, well-known technical solutions have not been shown or described in detail to avoid obscuring aspects of the invention.

The present invention provides a motion control method for a lower limb bone rehabilitation robot based on interference compensation. First, a nonlinear filter is used to estimate the angular rate of the rotation angle of the hip joint and the knee joint, thereby avoiding the use of an angular rate measuring device, so that Control economic cost down. Secondly, the load moment of the rehabilitation robot is estimated and compensated by means of force sensor measurement. Then, the system friction torque and nominal value are compensated by means of friction modeling and equivalent control. Finally, the disturbance observer method is used to compensate the unknown uncertainties of the hip joint and knee joint systems in real time and online, thereby greatly improving the comfort of the entire rehabilitation patient's assisted motion control.

Next, a motion control method for a lower limb bone rehabilitation robot based on interference compensation of the present invention will be further explained and described in conjunction with the accompanying drawings. Referring to Fig. 1, the motion control method of a lower limb bone rehabilitation robot based on interference compensation includes the following steps:

Step S10, install FUTEK LSB200 force sensors on the thigh bar and calf bar of the lower limb bone rehabilitation robot respectively, measure the load force of the hip joint and knee joint, and estimate the load moment of the hip joint and knee joint respectively according to the position of the sensor;

Specifically, first install a FUTEK LSB200 force sensor at position C of the lower limb bone rehabilitation robot as shown in Figure 2, measure the load force of the hip joint, denoted as _T1 ; measure the distance between hip joints A and C, denoted as l _a1 . Measure the distance between thigh rods A and D of the lower limb bone rehabilitation robot, denoted as l ₁ .

Secondly, install a FUTEK LSB200 force sensor at the position F of the lower limb bone rehabilitation robot shown in Figure 2 to measure the load force of the knee joint, denoted as T ₂ ; measure the distance between the knee joints D and F, denoted as _la2 . Measure the distance between the calf pole D and G of the lower limb bone rehabilitation robot, denoted as l ₂ .

Finally, the hip and knee load moments are estimated as follows:

M _f1 = l _a1 T ₁ ; M _f2 = l _a2 T ₂ ;

Among them, M _f1 is the estimated value of the hip joint load moment, and M _f2 is the estimated value of the hip joint load moment.

Step S20, install incremental orthogonal photoelectric encoders on the thigh bar and calf bar of the lower limb bone rehabilitation robot respectively, measure the hip joint rotation angle and knee joint rotation angle of the bone robot, and obtain the hip joint rotation angle according to the motion data of the human body gait. The joint angle error and the knee joint angle error are integrated separately to obtain an error integral signal;

Concretely, at first the incremental quadrature photoelectric encoder is installed at the position B of the lower limb bone rehabilitation robot shown in accompanying drawing 2, and the rotation angle of the hip joint is measured, denoted as q ₁ ; the incremental quadrature photoelectric encoder is installed at the E position , to measure the knee joint rotation angle, denoted as q ₂ .

Secondly, according to the motion data of the human gait, set the expected values of the hip joint rotation angle and the knee joint rotation angle as follows:

q _d1 =a ₁₁ sin(b ₁₁ p+c ₁₁ )+a ₁₂ sin(b ₁₂ p+c ₁₂ )+a ₁₃ sin(b ₁₃ p+c ₁₃ );

q _d2 =a ₂₁ sin(b ₂₁ p+c ₂₁ )+a ₂₂ sin(b ₂₂ p+c ₂₂ )+a ₂₃ sin(b ₂₃ p+c ₂₃ );

Where q _d1 is the expected value of the hip joint rotation angle, q _d2 is the expected value of the knee joint rotation angle, a ₁₁ , a ₁₂ , a ₁₃ , a ₂₁ , a ₂₂ , a ₂₃ , b ₁₁ , b ₁₂ , b ₁₃ , b ₂₁ , b ₂₂ , b ₂₃ , c ₁₁ , c ₁₂ , c ₁₃ , c ₂₁ , c ₂₂ , and c ₂₃ are the gait data of the human body, and its detailed design is shown in the case implementation later. p is the percentage of the gait cycle, which is calculated as follows:

p=t/T _aa -floor(t/T _aa );

Where t is the exercising time of the rehabilitated person, and T _aa is the average cycle of the rehabilitated person's exercise pace. floor(t/T) represents taking the integer part to the left, such as floor(3.5)=3, and the final result is 0≤p≤1.

Finally, according to the comparison between the measured value of the hip joint rotation angle and the expected value, the error signal of the hip joint rotation angle is obtained, denoted as e ₁ , and its calculation method is as follows:

e ₁ =q ₁ -q _d1 ;

According to the comparison between the measured value of the knee joint rotation angle and the expected value, an error signal of the knee joint rotation angle is obtained, denoted as e ₂ , and its calculation method is as follows:

e ₂ =q ₂ -q _d2 ;

Integrate according to the hip joint angle error signal to obtain the hip joint angle error integral signal, which is denoted as s ₁ , and its calculation method is as follows:

s ₁ =∫ e ₁ dt;

where dt means integrating the time signal.

Integrate according to the knee joint angle error signal to obtain the knee joint angle error integral signal, denoted as s ₂ , and its calculation method is as follows:

s ₂ =∫e ₂ dt;

where dt means integrating the time signal.

Step S30, constructing a nonlinear filter differentiator according to the measurement signals of the hip joint rotation angle and the knee joint rotation angle to obtain the hip joint angular rate signal and the knee joint angular rate estimation signal;

Specifically, first, according to the hip joint rotation angle measurement signal q ₁ , the following nonlinear filter differentiator is established to solve the hip joint rotation angular velocity estimation signal, denoted as D ₁ , and its calculation method is as follows:

D ₁ (n)=(q ₁ (n)-q _1a (n))/(T _a |q ₁ (n)-q _1a (n)|+ε ₁ );

Among them, q _1a (n) is the filtered hip joint rotation angle signal, and T _a and ε ₁ are constant parameters. For the detailed design, please refer to the case implementation later. D ₁ (n) is the nth data of D ₁ , and T is the time interval between data.

Secondly, according to the knee joint rotation angle measurement signal q ₂ , the following nonlinear filter differentiator is established to solve the knee joint rotation angular velocity estimation signal, denoted as D ₂ , and its calculation method is as follows:

D ₂ (n)=(q ₂ (n)-q _2a (n))/(T _b |q ₂ (n)-q _2a (n)|+ε ₂ );

Among them, q _2a (n) is the filtered knee joint rotation angle signal, and T _b and ε ₂ are constant parameters. For the detailed design, please refer to the case implementation later. D ₂ (n) is the nth data of D ₂ , and T is the time interval between data.

Step S40, measuring the weight of the thigh strut and the weight of the calf strut, and constructing the angular acceleration of the skeletal system according to the measured values of the hip joint angle and the knee joint angle, as well as the lengths of the thigh strut and the calf strut Matrix and angular velocity matrix, and perform inverse transformation to obtain the inverse matrix of angular acceleration of the skeletal system;

Specifically, firstly, measure the weight of the thigh strut, denoted as m ₁ , and measure the weight of the calf strut, denoted as m ₂ .

Secondly, according to the measured value of the hip joint angle and the measured value of the knee joint angle, the element values of the angular acceleration matrix of the skeletal system are calculated as follows:

Then, according to the element values of the angular acceleration matrix of the skeletal system, the angular acceleration matrix of the skeletal system is established, denoted as M, and its composition is as follows:

And solve the inverse matrix of the bone angular acceleration system, denoted as M ₀ , which satisfies M ₀ M=E, where E is an identity matrix. At the same time, the composition of _M0 is as follows:

Finally, according to the hip joint angle measurement value and knee joint angle measurement value and their angular velocity estimation values, the element values of the skeletal system angular velocity matrix are calculated as follows:

Π ₁₁ ＝-m ₂ l ₁ l ₂ cos(q ₂ )D ₂ ;

Π ₁₂ ＝-m ₂ l ₁ l ₂ cos(q ₂ )D ₂ /2;

Π ₂₁ ＝-m ₂ l ₁ l ₂ cos(q ₂ )D ₂ /2;

Π ₂₂ =0;

Construct the angular velocity matrix of the skeletal system according to the elements of the angular velocity matrix of the skeletal system, denoted as C, and its composition is as follows:

Step S50, calculate the equivalent control quantity of angular velocity according to the estimated value of the hip joint rotation angular velocity and the knee joint rotation angular velocity, and then calculate the gravity related quantity of the thigh bar and the gravity related quantity of the calf bar according to the physical structure data of the rehabilitation robot ;

Specifically, firstly, according to the estimated hip joint rotational angular velocity, knee joint rotational angular velocity estimated value and skeletal system angular velocity matrix, the angular velocity equivalent control quantity is calculated, which is denoted as H _0a , and its composition is as follows:

Among them, h _0a1 is the equivalent control quantity of hip joint angular velocity, and h _0a2 is the equivalent control quantity of knee joint angular velocity. H _0a is calculated as follows:

Wherein D ₁ is the estimated signal of the rotational angular velocity of the hip joint, and D ₂ is the estimated signal of the rotational angular velocity of the knee joint.

Secondly, according to the hip joint rotation angle and knee joint rotation angle signals and the physical structure data of the rehabilitation robot, the gravitational relative quantities of the thigh bar and the calf bar are calculated, denoted as g ₁ and g ₂ , and the calculation method is as follows:

g ₁ =-m ₁ gl ₁ sin(q ₁ )/2-m ₂ gl ₂ sin(q ₁ -q ₂ )/2-m ₂ gl ₁ sin(q ₁ );

g ₂ =-m ₂ gl ₂ sin(q ₁ -q ₂ )/2;

Where g is the gravitational acceleration constant, which can be taken as 9.8.

Step S60, according to the estimated value of hip joint and knee joint rotation angular rate, carry out compensation design for hip joint friction force and knee joint friction force, and then according to the described value of hip joint and knee joint load moment estimate and bone angular acceleration System inverse matrix, solve the equivalent control quantity of hip joint and knee joint;

Specifically, firstly, according to the estimated values D ₁ and D ₂ of the rotational angular rates of the hip joint and the knee joint, respectively calculate the compensation amount of the friction force of the hip joint and the knee joint, denoted as f _a1 and f _a2 respectively, and the calculation method as follows:

f _a1 =f _s λ ₁ +f _c (1-λ ₁ );

f _a2 =f _s λ ₂ +f _c (1-λ ₂ );

where f _s and f _c are the estimated values of the static friction coefficient and the dynamic friction coefficient, respectively.

Secondly, according to the estimated load moments M _f1 and M _f2 of the hip and knee joints, the equivalent control value of the angular velocity H _0a , the friction compensation values of the hip and knee joints f _a1 and f _a2 , and the gravity-related quantities of the thigh bar and the gravity of the calf bar The related quantities g ₁ and g ₂ are used to calculate the equivalent control quantities of the hip joint and the knee joint, which are respectively denoted as u _1e and u _2e , and the calculation method is as follows:

Step S70, according to the equivalent control quantities of the hip joint and the knee joint, construct the interference observer of the hip joint and the knee joint, respectively solve the states and estimated values of the interference of the hip joint and the knee joint;

Specifically, firstly, the hip joint disturbance observer is designed as follows:

x _12d =k _a1 D ₁ +k _b1 q ₁ ;

为髋关节转动角期望值q_d1的导数。其中D_1d的计算如下：Among them, k _a1 and k _b1 are constant value parameters, and their detailed design is shown in the following case implementation.

is the derivative of the expected value q _d1 of the hip joint rotation angle. where D _1d is calculated as follows:

D _1d =u _1e -Y ₁₁₀ u ₁ -Y ₁₂₀ u ₂ ;

Among them, u _1e is the equivalent control quantity of the hip joint, u ₁ is the control torque of the hip joint, u ₂ is the control torque of the knee joint, Y ₁₁₀ and Y ₁₂₀ are the elements of the inverse matrix M ₀ of the bone angular acceleration system, as follows:

即为髋关节干扰估计值，w_1a为髋关节干扰观测器状态。finally got

That is, the estimated value of the hip joint disturbance, w _1a is the state of the hip joint disturbance observer.

Second, the knee joint disturbance observer is designed as follows:

x _22d =k _a2 D ₂ +k _b2 q ₂ ;

为膝关节转动角期望值q_d2的导数。其中D_2d的计算如下：Among them, k _a2 and k _b2 are constant value parameters, and their detailed design is shown in the following case implementation.

is the derivative of the expected value q _d2 of the knee joint rotation angle. where D _2d is calculated as follows:

D _2d =u _2e -Y ₂₁₀ u ₁ -Y ₂₂₀ u ₂ ;

Among them, u _2e is the equivalent control quantity of the knee joint, u ₁ is the control torque of the hip joint, u ₂ is the control torque of the knee joint, Y ₂₁₀ and Y ₂₂₀ are the elements of the inverse matrix M ₀ of the bone angular acceleration system, as follows:

即为膝关节干扰估计值，w_2a为膝关节干扰观测器状态。finally got

That is, the estimated value of the knee joint disturbance, w _2a is the state of the knee joint disturbance observer.

Step S80, according to the estimated hip joint and knee joint interference value, the hip joint and knee joint rotation angle error amount, the error integral amount and the hip joint and knee joint rotation angular rate signal estimated value, perform linear combination to generate the hip joint and knee joint Knee joint control moment for ultimate lower limb skeletal rehabilitation robot motion control.

髋关节转动角误差量e₁、、误差积分量s₁、髋关节等效控制量u_1e与髋关节转动角速率信号估计值D₁进行线性组合，生成髋关节控制力矩u₁，其计算方式如下：Specifically, firstly, according to the estimated value of hip joint interference

The hip joint rotation angle error amount e ₁ , the error integral amount s ₁ , the hip joint equivalent control amount u _1e and the estimated value D ₁ of the hip joint rotation angle rate signal are linearly combined to generate the hip joint control torque u ₁ , and the calculation method is as follows:

Among them, k ₁₁ , k ₁₂ and k ₁₃ are constant value parameters, and their detailed design is shown in the following case implementation.

膝关节转动角误差量e₂、误差积分量s₂、膝关节等效控制量u_2e与膝关节转动角速率信号估计值D₂进行线性组合，生成膝关节控制力矩u₂，其计算方式如下：Second, according to the estimate of the knee joint disturbance

Knee joint rotation angle error e ₂ , error integral s ₂ , knee joint equivalent control value u _2e and knee joint rotation angle rate signal estimated value D ₂ are linearly combined to generate knee joint control torque u ₂ , which is calculated as follows :

Among them, k ₂₁ , k ₂₂ and k ₂₃ are constant value parameters, and their detailed design is shown in the following case implementation.

Finally, according to the generated hip joint control torque and knee joint control torque, it is sent to the lower limb bone rehabilitation robot system, which can assist the rehabilitation person to perform assisted walking.

Case implementation and computer simulation simulation results analysis

In order to verify the correctness and effectiveness of the method provided by the present invention, the following case simulation is provided for simulation.

In step S10, l ₁ =0.52 and l ₂ =0.42 are measured. In step S20, select T _aa =2, a ₁₁ =20, a ₁₂ =81, a ₁₃ =2.5, a 21 =34, a ₂₂ =8, a ₂₃ = ₂₃ , b ₁₁ =7.3, b ₁₂ =0.07 , b ₁₃ =17, b ₂₁ =2, b ₂₂ =15, b ₂₃ =11, c ₁₁ =1.1, c ₁₂ =-0.02, c ₁₃ =-4.7, c ₂₁ =-3.4, c ₂₂ =-1, c ₂₃ =-2.3. The expected value of the hip joint rotation angle is shown in Figure 3, the measured value of the hip joint rotation angle is shown in Figure 4, the comparison between the two is shown in Figure 5, the hip joint rotation error angle is shown in Figure 6, and the knee joint rotation angle The expected value is shown in Figure 7, the measured value of the knee joint rotation angle is shown in Figure 8, and the comparison between the two is shown in Figure 9. The knee joint rotation error angle is shown in Fig. 10. In step S30, T _a = 0.01, ε ₁ = 0.05, T _b = 0.01, ε ₂ = 0.05, T = 0.001 are selected to obtain the estimated signal of the rotational angular velocity of the hip joint as shown in Figure 11, and the estimated signal of the rotational angular velocity of the knee joint is shown in Figure 12 shows.

In step S40 and step S50, m ₁ =1.5 and m ₂ =1 are measured. In step S60, f _s =0.05 and f _c =0.05 are selected. In step S70, k _a1 =5, k _b1 =0.3, k _a2 =5, k _b2 =0.3 are selected to obtain the estimated hip joint interference as shown in FIG. 13 and the knee joint interference estimated value as shown in FIG. 14 .

In step S80, select k ₁₁ =255, k ₁₂ =20, k ₁₃ =2, k ₂₁ =255, k ₂₂ =20, k ₂₃ =2 to obtain the final hip joint control torque as shown in Figure 15, the knee The joint control torque is shown in Fig. 16.

It can be seen from Fig. 13 and Fig. 14 that the method provided by the present invention estimates the unknown disturbance torque and system uncertainty of the hip joint and knee joint, and the estimated value fluctuates with the progress of the gait. It can be seen from Fig. 5 and Fig. 9 that the method provided by the present invention can better track the desired gait, while Fig. 6 and Fig. 10 show the tracking error in the presence of system interference and uncertainty, which can be It can be seen that the error fluctuates around the value of 0, and it can be seen from the comparison curves given in Figure 5 and Figure 9 that the tracking effect can meet the needs of the rehabilitated. Figures 11 and 12 give estimates of the hip and knee rotational angular rates. However, the present invention only simply measures the rotation angles of the hip joint and the knee joint, and does not measure the angular rates of the two, but performs approximate estimation and solution of the digital filter differential. It can be seen that the estimated value can fully meet the control requirements of the rehabilitation robot, thereby saving the installation of angular velocity measurement components, making the whole method simple and economical. To sum up, the lower limb bone rehabilitation robot method provided by the present invention is completely feasible and has high engineering practical value.

Other embodiments of the invention will be readily apparent to those skilled in the art from consideration of the specification and practice of such an invention. This application is intended to cover any modification, use or adaptation of the present invention. These modifications, uses or adaptations follow the general principles of the present invention and include common knowledge or conventional technical means in the technical field not specified in the present invention . The specification and examples are to be considered exemplary only, with the true scope and spirit of the invention indicated by the appended claims.

Claims (1)

1. A lower limb skeletal rehabilitation robot based on interference compensation, characterized in that a FUTEK LSB200 type force sensor is installed on the thigh between the hip joint A and the knee joint D of the lower limb skeletal rehabilitation robot, and the position is denoted as C, and the measurement Hip joint load force, denoted as T ₁ ; measuring the distance between hip joints A and C, denoted as l _a1 ; measuring the distance between hip joint A and knee joint D of the lower limb bone rehabilitation robot, denoted as l ₁ ; Secondly, install the FUTEKLSB200 type force sensor on the calf of the lower limb bone rehabilitation robot, the installation position is denoted as F, and the load force of the knee joint is measured, denoted as T ₂ ; the distance between the knee joints D and F is measured, denoted as l _a2 ; measure the distance between the calf rod of the lower limb bone rehabilitation robot from the knee joint D to the heel G, denoted as _l2 ; estimate the load moment of the hip joint and knee joint as follows: M _f1 = l _a1 T ₁ ; M _f2 = l _a2 T ₂ ; Among them, M _f1 is the estimated value of the load moment of the hip joint, and M _f2 is the estimated value of the load moment of the hip joint; an incremental orthogonal photoelectric encoder is installed at the position B between the hip joint A and the knee joint D of the lower limb skeletal rehabilitation robot to measure the hip joint The joint rotation angle is denoted as q ₁ ; an incremental orthogonal photoelectric encoder is installed at a position between the D position and the F position of the knee joint, the position is denoted as E, and the knee joint rotation angle is measured, denoted as q ₂ , and according to According to the motion data of human gait, the hip joint angle error and the knee joint angle error are obtained, and are respectively integrated to obtain the error integration signal; then according to the hip joint rotation angle and knee joint rotation angle measurement signals, a nonlinear filter differential is constructed device to obtain the hip joint angular velocity signal and the knee joint angular velocity estimation signal; then measure the weight of the thigh strut and the weight of the calf strut, according to the hip joint rotation angle measurement value and the knee joint rotation angle measurement value, and the thigh strut The length of the skeletal system angular acceleration matrix and the angular velocity matrix are constructed, and the inverse transformation is performed to obtain the inverse matrix of the skeletal system angular acceleration; then according to the estimated value of the angular velocity of the hip joint rotation and the estimated value of the angular velocity of the knee joint rotation Calculate the equivalent control quantity of angular velocity, and then calculate the gravity-related quantities of thigh bar and calf bar according to the physical structure data of the rehabilitation robot; Force and knee joint friction force compensation design, and then according to the hip and knee joint load moment estimates and bone angular acceleration system inverse matrix, solve the hip joint and knee joint equivalent control quantity; then according to the hip joint Joint and knee joint equivalent control quantity, build hip joint and knee joint interference observer, respectively solve the hip joint and knee joint interference observer state and interference estimation value; finally 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 estimated value of the hip joint and knee joint rotation angular velocity signal are linearly combined to generate the hip joint and knee joint control torque to realize the final motion control of the lower limb bone rehabilitation robot; The specific signal solution method includes the following seven parts: In the first part, according to the hip joint angle error and the knee joint angle error, the integration is performed respectively, and the operation of obtaining the error integral signal is as follows: q _d1 =a ₁₁ sin(b ₁₁ p+c ₁₁ )+a ₁₂ sin(b ₁₂ p+c ₁₂ )+a ₁₃ sin(b ₁₃ p+c ₁₃ ); q _d2 =a ₂₁ sin(b ₂₁ p+c ₂₁ )+a ₂₂ sin(b ₂₂ p+c ₂₂ )+a ₂₃ sin(b ₂₃ p+c ₂₃ ); p=t/T _aa -floor(t/T _aa ); e ₁ =q ₁ -q _d1 ; e ₂ =q ₂ -q _d2 ; s ₁ =∫ e ₁ dt; s ₂ =∫e ₂ dt; Where q ₁ is the measured value of the hip joint rotation angle of the lower limb skeletal rehabilitation robot, q ₂ is the measured value of the knee joint rotation angle of the lower limb skeletal rehabilitation robot; q _d1 is the expected value of the hip joint rotation angle, and q _d2 is the expected value of the knee joint rotation angle , a ₁₁ , a ₁₂ , a ₁₃ , a ₂₁ , a ₂₂ , a ₂₃ , b _{11 , b 12 ,} b ₁₃ , b ₂₁ , b ₂₂ , b ₂₃ , c ₁₁ , c ₁₂ , c ₁₃ , c ₂₁ , c ₂₂ , c ₂₃ is the gait data of the human body; p is the percentage of the gait cycle, t is the exercise time of the rehabilitated person, T _aa is the average cycle of the rehabilitated person's movement pace; floor(t/T) means the integer part to the left , such as floor(3.5)=3, the final result is 0≤p≤1; e ₁ is the hip joint angle error signal, e ₂ is the knee joint angle error signal, s ₁ is the hip joint angle error integration signal, where dt represents the The time signal is integrated; s ₂ is the integral signal of the knee joint rotation angle error; In the second part, according to the measurement signals of the hip joint rotation angle and the knee joint rotation angle, a nonlinear filter differentiator is constructed to obtain the solution of the hip joint angular velocity signal and the estimated knee joint angular velocity signal as follows:

D ₁ (n)=(q ₁ (n)-q _1a (n))/(T _a |q ₁ (n)-q _1a (n)|+ε ₁ );

D ₂ (n)=(q ₂ (n)-q _2a (n))/(T _b |q ₂ (n)-q _2a (n)|+ε ₂ ); Among them, q ₁ is the measurement signal of hip joint rotation angle, D ₁ is the estimated signal of hip joint rotation angle velocity, q _1a (n) is the filtered hip joint rotation angle signal, T _a , ε ₁ , T _b , ε ₂ are constant parameters; q ₂ is the knee joint rotation angle measurement signal, D ₂ is the knee joint rotation angular velocity estimation signal, q _2a (n) is the filtered knee joint rotation angle signal, and T is the time interval between data; The third part is to measure the weight of the thigh strut and the weight of the calf strut, and build the skeleton according to the measured values of the hip joint rotation angle and knee joint rotation angle, as well as the length of the thigh strut and the length of the calf strut The angular acceleration matrix of the system and the angular velocity matrix are inversely transformed to obtain the inverse matrix of the angular acceleration of the skeletal system. The formula is as follows:

M ₀ M=E;

Π ₁₁ ＝-m ₂ l ₁ l ₂ cos(q ₂ )D ₂ ; Π ₁₂ ＝-m ₂ l ₁ l ₂ cos(q ₂ )D ₂ /2; Π ₂₁ ＝-m ₂ l ₁ l ₂ cos(q ₂ )D ₂ /2; Π ₂₂ =0;

Where m ₁ is the weight of the thigh strut, m ₂ is the weight of the calf strut, M is the angular acceleration matrix of the skeletal system, M ₀ is the inverse matrix of the angular acceleration of the skeletal system, E is the identity matrix; C is the angular velocity matrix of the skeletal system; In the fourth part, the angular velocity equivalent control quantity is calculated according to the estimated value of the hip joint rotational angular velocity and the knee joint rotational angular velocity, and then according to the physical structure data of the rehabilitation robot, the gravity-related quantity of the thigh bar and the gravity-related quantity of the calf bar are calculated. The solution method is as follows:

g ₁ =-m ₁ gl ₁ sin(q ₁ )/2-m ₂ gl ₂ sin(q ₁ -q ₂ )/2-m ₂ gl ₁ sin(q ₁ ); g ₂ =-m ₂ gl ₂ sin(q ₁ -q ₂ )/2; Where H _0a is the equivalent control quantity of angular velocity, D ₁ is the estimated signal of hip joint rotational angular velocity, D ₂ is the described signal of knee joint rotational angular velocity estimation, h _0a1 is the equivalent control quantity of hip joint angular velocity, h _0a2 is The equivalent control quantity of the knee joint angular velocity, g ₁ and g ₂ are the gravity-related quantities of the thigh bar and the calf bar gravity-related quantities, g is the gravitational acceleration constant, and the value is 9.8; The fifth part, according to the hip joint and knee joint rotation angular velocity estimation value, the compensation design for the hip joint friction force and the knee joint friction force, and then according to the hip joint joint joint joint load moment estimation value and the bone angular acceleration The system inverse matrix, the calculation method of the equivalent control quantity of the hip joint and the knee joint is as follows: f _a1 =f _s λ ₁ +f _c (1-λ ₁ ); f _a2 =f _s λ ₂ +f _c (1-λ ₂ );

M _f1 = l _a1 T ₁ ; M _f2 = l _a2 T ₂ ;

Among them, T ₁ and T ₂ are the measured values of the load force of the hip joint and the knee joint respectively, l _a1 is the distance between the hip joint and the installation position of the hip load force sensor, and l _a2 is the distance between the knee joint and the installation position of the knee load force sensor distance; D ₁ and D ₂ are the estimated rotational angular velocity of the hip joint and knee joint, f _a1 and f _a2 are the friction compensation amount of the hip joint and knee joint, f _s and f _c are the coefficients of static friction and dynamic friction respectively Estimated value; M _f1 and M _f2 are the estimated load moments of the hip joint and knee joint, H _0a is the equivalent control value of the angular velocity, g ₁ and g ₂ are the gravity-related quantities of the thigh bar and the calf bar gravity, u _1e and u _2e is the equivalent control amount of the hip joint and knee joint obtained from the final calculation; In the sixth part, according to the equivalent control quantities of the hip joint and the knee joint, a hip joint and knee joint interference observer is constructed, and the solution method for the state and interference estimation value of the hip joint and knee joint interference observer is as follows:

x _12d =k _a1 D ₁ +k _b1 q ₁ ;

D _1d =u _1e -Y ₁₁₀ u ₁ -Y ₁₂₀ u ₂ ;

x _22d =k _a2 D ₂ +k _b2 q ₂ ;

D _2d =u _2e -Y ₂₁₀ u ₁ -Y ₂₂₀ u ₂ ;

Among them k _a1 , k _b1 , k _a2 , k _b2 are constant parameters;

is the derivative of the expected value q _d1 of the hip joint rotation angle;

is the derivative of the expected value q _d2 of the knee joint rotation angle; u _1e is the equivalent control quantity of the hip joint, u ₁ is the control torque of the hip joint, u _2e is the equivalent control quantity of the knee joint, u ₂ is the control torque of the knee joint, Y ₁₁₀ , Y ₁₂₀ , Y ₂₁₀ , and Y ₂₂₀ are the elements of the inverse matrix M ₀ of the bone angular acceleration system, and finally get

is the estimated value of the hip joint disturbance, w _1a is the state of the hip joint disturbance observer;

is the estimated value of the knee joint disturbance, w _2a is the state of the knee joint disturbance observer; In the seventh part, linear combination is performed 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 rotational angular velocity signal of the hip joint and the knee joint to generate the hip joint and knee joint The method of controlling the torque of the knee joint is as follows:

in

and

are the estimated values of interference between the hip joint and the knee joint, respectively, e ₁ and e ₂ are the rotational angle errors of the hip joint and the knee joint respectively, s ₁ and s ₂ are the integral rotational angle errors of the hip joint and the knee joint respectively, u _1e and u _2e are the equivalent control quantities of the hip joint and knee joint, respectively, D ₁ and D ₂ are the estimated values of the rotational angular velocity signals of the hip joint and knee joint, respectively, and u ₁ and u ₂ are the final control torques of the hip joint and knee joint , k ₂₁ , k ₂₂ and k ₂₃ , k ₁₁ , k ₁₂ and k ₁₃ are constant value parameters; the generated hip joint control torque and knee joint control torque are sent to the lower limb bone rehabilitation robot system to realize the Movement and Control.

Images