Lumbar rehabilitation flexible exoskeleton robot force / position hybrid control method based on LESO-SMC

Abstract

The invention belongs to the technical field of rehabilitation robot control, and particularly relates to a lumbar rehabilitation flexible exoskeleton robot force / position hybrid control method based on LESO-SMC, and the method comprises the following steps: S1, building a lumbar rehabilitation flexible exoskeleton robot kinematics model; s2, establishing a kinetic model of the 4SPU-SP bionic parallel mechanism; s3, rigid body dynamics of the four-degree-of-freedom rehabilitation robot is described as a second-order system; s4, the change of the waist posture is tracked and detected in real time to achieve follow-up control of the lumbar vertebra rehabilitation flexible exoskeleton, and therefore position control is achieved; s5, solving an output force target value Fqd of the electric push rod as system output by taking the attitude angle of the movable platform and the target support assisting force F required by the patient as system input, so as to realize force control; according to the invention, the linear expansion state observer is introduced to estimate the external interference and internal uncertainty of the rehabilitation robot, and the estimation error output by the system is introduced into the sliding mode controller, so that the stability of the whole closed-loop system is ensured.

Description

Technical Field

[0001] The invention belongs to the technical field of rehabilitation robot control, and in particular relates to a force / position hybrid control method of a lumbar rehabilitation flexible exoskeleton robot based on LESO-SMC. Background Art

[0002] The Chinese invention patent with the patent number of 2020108154959 discloses a wearable lumbar rehabilitation exoskeleton robot, which must not only provide stable support and assistance to the lumbar spine, but also be able to follow the real-time movement of the human waist. In actual use, the lumbar rehabilitation exoskeleton robot is not a rigid platform, and there are errors in the kinematic and dynamic modeling; the whole system has a nonlinear structure of soft tissue power transmission, and there are many unknown interferences and uncertainties; inertial measurement sensors, displacement sensors, and tension and pressure sensors will also produce errors due to changes in temperature and humidity. If it is controlled only based on mathematical models, it will affect the user experience of the lumbar rehabilitation flexible exoskeleton robot, and it has been verified that the traditional PID control scheme is used for control, which has a poor suppression effect on some high-frequency noise. Summary of the invention

[0003] The purpose of the present invention is to overcome the defects of a wearable lumbar rehabilitation exoskeleton robot in the prior art and to provide a force / position hybrid control method for a lumbar rehabilitation flexible exoskeleton robot based on LESO-SMC.

[0004] The technical solution adopted by the present invention to solve its technical problem is:

[0005] A force / position hybrid control method for a lumbar rehabilitation flexible exoskeleton robot based on LESO-SMC, characterized in that the exoskeleton robot comprises a plurality of lower support frames located on the same horizontal plane and a plurality of upper support frames located on the same horizontal plane, the plurality of lower support frames are connected by a lower flexible adjustment belt for being worn outside the pelvis; the plurality of upper support frames are connected by an upper flexible adjustment belt for being worn outside the lower ribs; a lumbar curvature support assembly is arranged at the rear waist position between the lower flexible adjustment belt and the upper flexible adjustment belt, four electric push rods are arranged in parallel between the lower support frame and the upper support frame, the lower end of the electric push rod is connected to the lower support frame by a ball joint, and the upper end thereof is connected to the upper support frame by a universal joint; the upper support frame and the upper flexible adjustment belt form a The movable platform, the lower support frame and the lower flexible adjustment belt form a fixed platform; the four electric push rods form a 4SPU parallel mechanism with the fixed platform and the movable platform; a linear displacement sensor for real-time detection of the extension length of each electric push rod is arranged on each electric push rod; the first inertial measurement sensor IMU1 is installed on the inner side of the lower flexible adjustment belt; the second inertial measurement sensor IMU2 is installed on the inner side of the upper flexible adjustment belt; when installed, the two sensors correspond to each other up and down and are located at the human spine; a tension and pressure sensor is arranged at the position where the inner side of the lower flexible adjustment belt contacts the bony protrusion on the upper edge of the pelvis, and a tension and pressure sensor is also arranged at the position where the inner side of the upper flexible adjustment belt contacts the bony protrusion on the lower edge of the ribs; the lumbar rehabilitation flexible exoskeleton robot cooperates with the lumbar movement to form a 4SPU-SP bionic parallel mechanism;

[0006] The force / position hybrid control method comprises the following steps:

[0007] S1. Establish the kinematic model of the flexible exoskeleton robot for lumbar rehabilitation;

[0008] {O} = {O-XYZ} is defined as the base coordinate system, which represents the coordinate system fixed on the fixed platform; and {p} = {p-xyz} is defined as the motion coordinate system, which represents the coordinates on the moving platform; l1...l4 represent the drive rods, i.e., the electric push rods, each of which consists of a motion rod l Bi and piston rod mi l0 represents the spine of the body, which can be regarded as a passive rod; the ends of the lower platform are S pairs, denoted as S i (i=1,2,3,4), each end point of the upper platform is a U pair, denoted as U i (i=1,2,3,4); the auxiliary coordinate system can be defined along with the driving rod ij ,i=1...4,j=1...7, where i is the number of driving rods;

[0009] The lengths of the four electric push rods are the input variables of the exoskeleton robot, denoted as l i(i=1,2,3,4), the output variable of the robot is the position and posture of the moving platform, that is, the roll angle of the moving platform rotating around the X-axis of the fixed coordinate system, the pitch angle of the moving platform rotating around the Y-axis, and the deflection angle of the moving platform rotating around the Z-axis, which are denoted as γ, β, and α respectively. The position and posture parameters of the moving platform can be expressed as q=[pz,α,β,γ] T , where p z is the Z-axis coordinate of the origin p of the moving coordinate system {p-xyz} in the fixed coordinate system {O-XYZ}, and its value is the initial rod length of the virtual constraint branch SP, which is determined by the vertical dimension between the wearer's lower chest and hip circumference; the length and width of the moving platform are respectively expressed as b h and b s Indicates that the length and width of the fixed platform are distributed by a h and a s Indicates that its value is determined by the wearer's underbust and hip measurements;

[0010] The endpoints S of the given platform B can be obtained i In the fixed platform coordinate system {O b =O b -X b Y b Z b The following coordinates represent:

[0011]

[0012] The Euler transformation method is used to represent the position and posture of the lumbar rehabilitation flexible exoskeleton bionic parallel mechanism. The position vector of the origin p of the moving coordinate system {p-xyz} relative to the fixed coordinate system {O} is set to be p = [p x ,p y ,p z ], the posture of the motion platform is expressed by the zyx-type Euler angle, then the rotation matrix of the motion platform in the fixed coordinate system {O} is:

[0013]

[0014] Where c j =cosj,s j = sinj,j = α, β, γ

[0015] Bionic parallel mechanism moving platform m each end point U i The position vector in the moving coordinate system {p-xyz} is

[0016]

[0017] According to formula (1-2), (1-3), we can get U i The position vector of the point in the fixed coordinate system {O} is

[0018]

[0019] The initial rod length of the constraint branch SP can be determined according to the initial distance between the moving platform and the fixed platform, that is, the Z-axis coordinate p of the origin p of the moving coordinate system in the fixed coordinate system {O-XYZ} z ,

[0020]

[0021]

[0022] According to l i =| O U i - O S i |. The length expressions of each driving branch rod can be obtained:

[0023]

[0024] This is the inverse solution of the kinematic position of the exoskeleton robot. If the attitude angle (α, β, γ) of the moving platform relative to the fixed platform is known, the length of the four telescopic rods required can be obtained;

[0025] The attitude angle measured by the first inertial measurement sensor IMU1 is The attitude angle measured by the first inertial measurement sensor IMU2 is Then the relative posture of the human waist, that is, the posture angle (α, β, γ) of the bionic parallel mechanism moving platform relative to the fixed platform can be expressed as

[0026]

[0027] S2. Establish the dynamic model of 4SPU-SP bionic parallel mechanism;

[0028] Assume G0, f0 and n0 are the gravity vector, inertia force and inertia moment of the dynamic platform of the lumbar rehabilitation flexible exoskeleton bionic parallel mechanism. The dynamic platform includes the upper body of the human body. m0 represents the mass of the dynamic platform of the bionic parallel mechanism, which can be approximated as the mass of the upper body, which is about two-thirds of the human body weight. Assume G Bi 、f Bi and n Bi is the gravity vector, inertia force and inertia moment of the electric push rod swing rod, G mi 、f mi and n mi is the gravity vector, inertia force and inertia moment of the piston rod, m Bi and m mi are the masses of the swing rod and telescopic rod respectively;

[0029]

[0030] In the formula,m I0, i I Bi and m I Bi are the inertia matrices of the parallel mechanism moving platform, swing rod and telescopic rod relative to the center of mass coordinate system; is the rotation matrix of the centroid coordinate system of the i-th driving branch relative to the fixed coordinate system; the inertia parameters are obtained by Solidworks software based on the model measurement; according to the principle of virtual work, the sum of the external force / torque and inertia force / torque of each component of the parallel robot and the virtual work done by the driving force is zero, so the dynamic equation of the bionic parallel mechanism can be described as

[0031]

[0032] In the formula, F q =[F T T T ] T =[F l1 ... F l4 ] T is the vector of driving force in 4-SPU / SP bionic parallel mechanism; V r =J m V,[v Bi ω Bi ] T =J Bi V,[v mi ω mi ] T =J mi V is put into formula (2-2) to get

[0033]

[0034] That is the inverse dynamics model of the 4SPU-SP bionic parallel mechanism;

[0035] S3. Through forward dynamics analysis, the rigid body dynamics of the four-degree-of-freedom rehabilitation robot is described as a second-order system; the inverse dynamics model of the bionic parallel mechanism of formula (2-3) is converted into Lagrangian form and described as a second-order nonlinear differential equation, which is expressed as follows:

[0036]

[0037] Where q is the required drive rod length vector, M(q) is the exoskeleton system mass matrix, represents the Coriolis term and the centripetal term, G(q) represents the gravitational effect of the system's moving platform and upper body, is the unknown disturbance, F q is a vector composed of the forces generated by the four driving electric push rods, τc The force vector representing the 4 degrees of freedom; and τ p Respectively represent the control torque of the force controller and the position controller on the electric push rod;

[0038] Considering the modeling error of the bionic parallel mechanism,

[0039]

[0040] in and is M(q) and The estimated value of the known part, ΔM(q) and ΔC(q) are the corresponding estimation errors; so the dynamic model differential equation (2-1) can be written as

[0041]

[0042] in,

[0043]

[0044] The dynamic model (3-3) of the four-degree-of-freedom bionic parallel mechanism can be expressed as follows:

[0045]

[0046] Taking the first electric push rod subsystem of the lumbar rehabilitation flexible exoskeleton bionic parallel mechanism as an example, its dynamic differential equation can be expressed as

[0047]

[0048] Can be written as

[0049]

[0050] in

[0051]

[0052] Assumptions The derivative of exists, and The derivative of is bounded; let q=y1=x1, The total disturbance is the expanded state variable Its time derivative is Then (3-7) can be written in the state space as

[0053]

[0054] Based on (3-9), the linear extended state observer LESO can establish the following formula:

[0055]

[0056] Where p1, p2 and p3 are LESO parameters. By properly choosing the parameters in (3-10), the output of LESO is The states x1, x2, and x3 can be tracked;

[0057] Taking the first degree of freedom in the dynamic model (3-5) as an example, define the sliding plane

[0058]

[0059] Where c1>0 is a known parameter, is the output of the linear extended state observer LESO, y 1d is a bounded expected output, then the synovial controller is:

[0060]

[0061] Considering the stability of the sliding mode controller SMC, the following Lyapunov function is obtained:

[0062]

[0063] According to the definition of sliding surface in (3-11), the differential of s can be given as:

[0064]

[0065] in is the LESO error generated by tracking the actual output, is the estimated value of y1; review From the definition of , we can get the time derivative of V as

[0066]

[0067] The above formula can be written as

[0068]

[0069] By definition The following formula can be obtained

[0070]

[0071] It is not difficult to find

[0072]

[0073] If k1>1 / 2,

[0074]

[0075] so

[0076]

[0077] It can be seen that the tracking error of the closed-loop system converges to the bounded zero neighborhood, and the convergence speed of the system is determined by the sliding mode parameter k1;

[0078] S4. Obtain the waist movement posture through waist movement intention perception based on inertial measurement sensor, and track and detect the change of waist posture in real time to realize the follow-up control of lumbar rehabilitation flexible exoskeleton, so as to achieve position control;

[0079] The inverse kinematics model (1-8) is converted into a mathematical model of the control system, specifically:

[0080]

[0081] Where, i = 1, 2, 3, 4; f i1 、f i2 、f i3 It is a function related to the coordinate axes X, Y, and Z, and its expression is about the variables (α, β, γ, p z )’s trigonometric functions;

[0082] When the human waist motion posture is given by the inertial measurement sensor, the length of the branch electric push rod driven by the lumbar rehabilitation flexible exoskeleton bionic parallel mechanism can be uniquely solved; when the lumbar rehabilitation flexible exoskeleton is in follow-up decompression and active rehabilitation training, the inertial measurement sensor tracks and detects the changes in waist posture Δα, Δβ, Δγ in real time, and substitutes them into formula (4-1) to solve the expected target length of each electric push rod:

[0083]

[0084] The displacement sensor integrated in the electric push rod provides real-time feedback of the actual rod length l i , the error ΔL between the actual length of the electric push rod and the expected rod length i = l i -L i , as the input of LESO-SMC position controller, it is sent to the linear extended state observation sliding mode controller for error estimation and compensation, and the output position control torque τ of the electric push rod p The electric push rod driver outputs the corresponding voltage to control the push rod movement, thus realizing the follow-up control of the lumbar rehabilitation flexible exoskeleton;

[0085] S5. The posture angle of the dynamic platform and the target support force F required by the patient are obtained by sensing the waist movement intention based on the inertial measurement sensor as system input, and then substituted into the inverse dynamics model (2-3) to solve the target value F of the electric push rod output force qdAs the output of the system, it is controlled by the force controller to achieve force control;

[0086] The inverse dynamics model of the lumbar spine rehabilitation flexible exoskeleton is written in Lagrangian form:

[0087]

[0088] Where, q = [α β γ F], represents the input of the lumbar rehabilitation flexible exoskeleton system; D(q) is the inertia matrix of the robot system, which is the driving force required to achieve the corresponding acceleration of the mechanism; is the driving force that the robot system needs to provide to overcome the centrifugal force and Coriolis force vector and other velocity factors; G(q) is the driving force that the robot system needs to provide to overcome the gravity factor of the mechanism; τ is the external interference force acting on the lumbar rehabilitation flexible exoskeleton system; (J T ) + is the pseudo-inverse matrix of the Jacobian matrix J;

[0089] The expected target value F of the electric push rod output force is obtained through the dynamic model of the lumbar rehabilitation flexible exoskeleton qd The expected target push rod output force is compared with the actual push rod driving force f fed back by the pull-up pressure sensor of each push rod in real time. qd For comparison, the error ΔF is input into the LESO-SMC force controller, which estimates and compensates for it based on factors such as external interference, and sends the control torque of the electric push rod to the driver of the electric push rod, thereby controlling the output force of the electric push rod and realizing the function of the lumbar rehabilitation flexible exoskeleton to provide patients with stable lumbar support assistance.

[0090] Furthermore, the response frequency of the force controller is much greater than the response frequency of the position controller; the adjustment of the force control to the position control is based on the error ΔF of the target support assistance and the allowable error threshold, which is specifically divided into three cases: where η is the adjustment threshold and ε is the safe parking threshold;

[0091] a) When the error ΔF≤η of the target support assistance changes within a small range, the high-level force controller does not participate in the control, and the position controller independently controls the lumbar rehabilitation flexible exoskeleton to maintain follow-up;

[0092] b) When the error of the target support force η<ΔF≤ε, but changes within a certain safety range, the high-level force controller moderately adjusts the position control to make the lumbar rehabilitation flexible exoskeleton system return to normal as soon as possible, and then allows the position controller to independently control and maintain the follow-up;

[0093] c) When the error of the target support assistance exceeds the preset safety threshold, that is, ΔF>ε, the high-level force controller enables the system to stop safely as soon as possible to avoid unpredictable safety accidents and harm to the patient.

[0094] The beneficial effects of the force / position hybrid control method of the lumbar rehabilitation flexible exoskeleton robot based on LESO-SMC of the present invention are:

[0095] The patent of this invention adopts a force-position hybrid control scheme, and improves the control performance of the overall control system by combining forward and inverse kinematics. The rehabilitation robot can quickly track and support the upper body of the human body by performing force-position hybrid control on the electric push rod. The invention introduces a linear extended state observer to estimate the external interference and internal uncertainty of the rehabilitation robot, and introduces the estimated error of the system output into the sliding mode controller, ensuring the stability of the overall closed-loop system. BRIEF DESCRIPTION OF THE DRAWINGS

[0096] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0097] Figure 1 is a three-dimensional structural diagram of a lumbar vertebra rehabilitation flexible exoskeleton robot according to an embodiment of the present invention;

[0098] Figure 2 It is a position diagram of the first inertial measurement sensor IMU1 and the second inertial measurement sensor IMU2 corresponding to the human body after the exoskeleton robot according to the embodiment of the present invention is worn;

[0099] Figure 3 This is a diagram showing the positions of the four tension and pressure sensors corresponding to the human body when the exoskeleton robot according to an embodiment of the present invention is worn.

[0100] Figure 4 It is a structural diagram of the lumbar rehabilitation flexible exoskeleton robot simplified into a 4SPU-SP bionic parallel mechanism;

[0101] Figure 5 This is a schematic diagram of the 4SPU-SP bionic parallel mechanism and a schematic diagram of the coordinate system;

[0102] Figure 6 is a block diagram of the LESO-SMC design of an embodiment of the present invention;

[0103] Figure 7 It is the control block diagram of the four electric actuator subsystems;

[0104] Figure 8 It is a control strategy block diagram of the lumbar rehabilitation flexible exoskeleton robot;

[0105] Fig. 9 It is a control flow chart of the two working modes of the lumbar rehabilitation flexible exoskeleton robot.

[0106] In the figure: 1. lower support frame, 2. upper support frame, 3. lower flexible adjustment belt, 4. upper flexible adjustment belt, 5. lumbar curvature support assembly, 6. electric push rod. DETAILED DESCRIPTION

[0107] The present invention will now be described in further detail with reference to the accompanying drawings. These drawings are simplified schematic diagrams, which only illustrate the basic structure of the present invention in a schematic manner, and therefore only show the components related to the present invention.

[0108] like Figure 1 As shown, the lumbar rehabilitation flexible exoskeleton robot of the present invention includes a plurality of lower support frames 1 located on the same horizontal plane and a plurality of upper support frames 2 located on the same horizontal plane, the plurality of lower support frames 1 are connected by a lower flexible adjustment belt 3 for being worn outside the pelvis; the plurality of upper support frames 2 are connected by an upper flexible adjustment belt 4 for being worn outside the lower ribs; a lumbar curvature support assembly 5 is arranged at the back waist position between the lower flexible adjustment belt 3 and the upper flexible adjustment belt 4, four electric push rods 6 are arranged in parallel between the lower support frame 1 and the upper support frame 2, the lower end of the electric push rod 6 is connected to the lower support frame 1 through a ball joint, and the upper end thereof is connected to the upper support frame 2 through a universal joint; the upper support frame 2 and the upper flexible adjustment belt 4 form a moving platform, and the lower support frame 1 and the lower flexible adjustment belt 3 form a fixed platform; the four electric push rods 6 form a 4SPU parallel mechanism with the fixed platform and the moving platform; a linear displacement sensor for real-time detection of the extension length of each electric push rod 6 is arranged on each electric push rod 6. The first inertial measurement sensor IMU1 is installed on the inner side of the lower flexible adjustment belt 3; the second inertial measurement sensor IMU2 is installed on the inner side of the upper flexible adjustment belt 4; when mounted, the two sensors correspond to each other up and down and are located at the human spine position, see Figure 2 See also Figure 3 A tension and pressure sensor is provided at the position where the inner side of the lower flexible adjustment belt 3 contacts the bony protrusion at the upper edge of the pelvis, and a tension and pressure sensor is also provided at the position where the inner side of the upper flexible adjustment belt 4 contacts the bony protrusion at the lower edge of the ribs. The parallel mechanism of the lumbar rehabilitation flexible exoskeleton robot of the present invention can be simplified as follows: Figure 4 As shown, the fixed platform and the moving platform are connected by 4 SPU driving branches and the virtual SP slave branch in the middle, and the virtual slave branch SP is equivalent to the waist of the human body. The lumbar rehabilitation flexible exoskeleton robot cooperates with the lumbar movement to form a 4SPU-SP bionic parallel mechanism.

[0109] The force / position hybrid control method comprises the following steps:

[0110] S1. Establish the kinematic model of the flexible exoskeleton robot for lumbar rehabilitation;

[0111] See also Figure 5, define {O} = {O-XYZ} as the base coordinate system, which represents the coordinate system fixed on the fixed platform; and {p} = {p-xyz} is defined as the motion coordinate system, which represents the coordinates on the moving platform; l1...l4 represent the drive rods (i.e. electric push rods), each of which consists of a motion rod l Bi and piston rod mi l0 represents the spine of the body, which can be regarded as a passive rod; the end points of the lower platform are S pairs, denoted as S i (i=1,2,3,4), each end point of the upper platform is a U pair, denoted as U i (i=1,2,3,4). Auxiliary coordinate system can be defined along with the driving rod. ij ,i=1...4,j=1...7, where i is the number of driving rods.

[0112] The lengths of the four electric push rods are the input variables of the exoskeleton robot, denoted as l i (l=1,2,3,4), the output variable of the robot is the position and posture of the moving platform, that is, the roll angle of the moving platform rotating around the X-axis of the fixed coordinate system, the pitch angle of the moving platform rotating around the Y-axis, and the deflection angle of the moving platform rotating around the Z-axis, which are denoted as γ, β, and α respectively. The position and posture parameters of the moving platform can be expressed as q=[pz,α,β,γ] T , where p z is the Z-axis coordinate of the origin p of the moving coordinate system {p-xyz} in the fixed coordinate system {O-XYZ}, and its value is the initial rod length of the virtual constraint branch SP, which is determined by the vertical dimension between the wearer's lower chest and hip circumference. The length and width of the moving platform are represented by b h and b s Indicates that the length and width of the fixed platform are distributed by a h and a s Indicates that its value is determined by the wearer's underbust and hip measurements.

[0113] The endpoints S of the given platform B can be obtained i In the fixed platform coordinate system {O b =O b -X b Y b Z b The following coordinates represent:

[0114]

[0115] The Euler transformation method is used to represent the position and posture of the lumbar rehabilitation flexible exoskeleton bionic parallel mechanism. The position vector of the origin p of the moving coordinate system {p-xyz} relative to the fixed coordinate system {O} is set to be p = [p x ,p y ,p z], the posture of the motion platform is expressed by the zyx-type Euler angle, then the rotation matrix of the motion platform in the fixed coordinate system {O} is:

[0116]

[0117] Where c j =cosj,s j = sinj,j = α, β, γ

[0118] Bionic parallel mechanism moving platform m each end point U i The position vector in the moving coordinate system {p-xyz} is

[0119]

[0120] According to formula (1-2), (1-3), we can get U i The position vector of the point in the fixed coordinate system {O} is

[0121]

[0122] The initial rod length of the constraint branch SP can be determined according to the initial distance between the moving platform and the fixed platform, that is, the Z-axis coordinate p of the origin p of the moving coordinate system in the fixed coordinate system {O-XYZ} z ,

[0123]

[0124]

[0125] According to l i =| O U i - O S i |. The length expressions of each driving branch rod can be obtained:

[0126]

[0127] This is the inverse solution of the kinematic position of the exoskeleton robot. If the attitude angle (α, β, γ) of the moving platform relative to the fixed platform is known, the length of the four telescopic rods required can be obtained;

[0128] The attitude angle measured by the first inertial measurement sensor IMU1 is The attitude angle measured by the first inertial measurement sensor IMU2 is Then the relative posture of the human waist, that is, the posture angle (α, β, γ) of the bionic parallel mechanism moving platform relative to the fixed platform can be expressed as

[0129]

[0130] S2. Establish the dynamic model of 4SPU-SP bionic parallel mechanism;

[0131] Assume G0, f0 and n0 are the gravity vector, inertia force and inertia moment of the dynamic platform of the lumbar rehabilitation flexible exoskeleton bionic parallel mechanism. The dynamic platform includes the upper body of the human body. m0 represents the mass of the dynamic platform of the bionic parallel mechanism, which can be approximated as the mass of the upper body, which is about two-thirds of the human body weight. Assume G Bi 、f Bi and n Bi is the gravity vector, inertia force and inertia moment of the electric push rod swing rod, G mi 、f mi and n mi is the gravity vector, inertia force and inertia moment of the piston rod, m Bi and m mi are the masses of the swing rod and telescopic rod respectively;

[0132]

[0133] In the formula, m I0, i I Bi and m I Bi are the inertia matrices of the parallel mechanism moving platform, swing rod and telescopic rod relative to the center of mass coordinate system; is the rotation matrix of the centroid coordinate system of the i-th driving branch relative to the fixed coordinate system; the inertia parameters are obtained by Solidworks software based on the model measurement; according to the principle of virtual work, the sum of the external force / torque and inertia force / torque of each component of the parallel robot and the virtual work done by the driving force is zero, so the dynamic equation of the bionic parallel mechanism can be described as

[0134]

[0135] In the formula, F q =[F T T T ] T =[F l1 ... F l4 ] T is the vector of driving force in 4-SPU / SP bionic parallel mechanism; V r =J m V,[v Bi ω Bi ] T =J Bi V,[v mi ω mi ] T =J mi V is put into formula (2-2) to get

[0136]

[0137] That is the inverse dynamics model of the 4SPU-SP bionic parallel mechanism;

[0138] S3. Through forward dynamics analysis, the rigid body dynamics of the four-degree-of-freedom rehabilitation robot is described as a second-order system; the inverse dynamics model of the bionic parallel mechanism of formula (2-3) is converted into Lagrangian form and described as a second-order nonlinear differential equation, which is expressed as follows:

[0139]

[0140] Where q is the required drive rod length vector, M(q) is the exoskeleton system mass matrix, represents the Coriolis term and the centripetal term, G(q) represents the gravitational effect of the system's moving platform and upper body, is the unknown disturbance, F q is a vector composed of the forces generated by the four driving electric push rods, τ c The force vector representing the 4 degrees of freedom; and τ p Respectively represent the control torque of the force controller and the position controller on the electric push rod;

[0141] Considering the modeling error of the bionic parallel mechanism,

[0142]

[0143] in and is M(q) and The estimated value of the known part, ΔM(q) and ΔC(q) are the corresponding estimation errors; so the dynamic model differential equation (2-1) can be written as

[0144]

[0145] in,

[0146]

[0147] The dynamic model (3-3) of the four-degree-of-freedom bionic parallel mechanism can be expressed as follows:

[0148]

[0149] Taking the first electric push rod subsystem of the lumbar rehabilitation flexible exoskeleton bionic parallel mechanism as an example, its dynamic differential equation can be expressed as

[0150]

[0151] Can be written as

[0152]

[0153] in

[0154]

[0155] Assumptions The derivative of exists, and The derivative of is bounded; let q=y1=x1, The total disturbance is the expanded state variable Its time derivative is Then (3-7) can be written in the state space as

[0156]

[0157] Based on (3-9), the linear extended state observer LESO can establish the following formula:

[0158]

[0159] Where p1, p2 and p3 are LESO parameters. By properly choosing the parameters in (3-10), the output of LESO is The states x1, x2, and x3 can be tracked;

[0160] Taking the first degree of freedom in the dynamic model (3-5) as an example, define the sliding plane

[0161]

[0162] Where c1>0 is a known parameter, is the output of the linear extended state observer LESO, y 1d is a bounded expected output, then the synovial controller is:

[0163]

[0164] Considering the stability of the sliding mode controller SMC, the following Lyapunov function is obtained:

[0165]

[0166] According to the definition of sliding surface in (3-11), the differential of s can be given as:

[0167]

[0168] in is the LESO error generated by tracking the actual output, is the estimated value of y1; review From the definition of , we can get the time derivative of V as

[0169]

[0170] The above formula can be written as

[0171]

[0172] By definition The following formula can be obtained

[0173]

[0174] It is not difficult to find

[0175]

[0176] If k1>1 / 2,

[0177]

[0178] so

[0179]

[0180] It can be seen that the tracking error of the closed-loop system converges to the bounded zero neighborhood, and the convergence speed of the system is determined by the sliding mode parameter k1;

[0181] S4. Obtain the waist movement posture through waist movement intention perception based on inertial measurement sensor, and track and detect the change of waist posture in real time to realize the follow-up control of lumbar rehabilitation flexible exoskeleton, so as to achieve position control;

[0182] The inverse kinematics model (1-8) is converted into a mathematical model of the control system, specifically:

[0183]

[0184] Where, i = 1, 2, 3, 4; f i1 、f i2 、f i3 It is a function related to the coordinate axes X, Y, and Z, and its expression is about the variables (α, β, γ, p z )’s trigonometric functions;

[0185] When the human waist motion posture is given by the inertial measurement sensor, the length of the branch electric push rod driven by the lumbar rehabilitation flexible exoskeleton bionic parallel mechanism can be uniquely solved; when the lumbar rehabilitation flexible exoskeleton is in follow-up decompression and active rehabilitation training, the inertial measurement sensor tracks and detects the changes in waist posture Δα, Δβ, Δγ in real time, and substitutes them into formula (4-1) to solve the expected target length of each electric push rod:

[0186]

[0187] The displacement sensor integrated in the electric push rod provides real-time feedback of the actual rod length l i , the error ΔL between the actual length of the electric push rod and the expected rod length i = l i -L i , as the input of LESO-SMC position controller, it is sent to the linear extended state observation sliding mode controller for error estimation and compensation, and the output position control torque τ of the electric push rod p The electric push rod driver outputs the corresponding voltage to control the push rod movement, thus realizing the follow-up control of the lumbar rehabilitation flexible exoskeleton;

[0188] S5. The posture angle of the dynamic platform and the target support force F required by the patient are obtained by sensing the waist movement intention based on the inertial measurement sensor as system input, and then substituted into the inverse dynamics model (2-3) to solve the target value F of the electric push rod output force qd As the output of the system, it is controlled by the force controller to achieve force control;

[0189] The inverse dynamics model of the lumbar spine rehabilitation flexible exoskeleton is written in Lagrangian form:

[0190]

[0191] Where, q = [α β γ F], represents the input of the lumbar rehabilitation flexible exoskeleton system; D(q) is the inertia matrix of the robot system, which is the driving force required to achieve the corresponding acceleration of the mechanism; is the driving force that the robot system needs to provide to overcome the centrifugal force and Coriolis force vector and other velocity factors; G(q) is the driving force that the robot system needs to provide to overcome the gravity factor of the mechanism; τ is the external interference force acting on the lumbar rehabilitation flexible exoskeleton system; (J T ) + is the pseudo-inverse matrix of the Jacobian matrix J;

[0192] The expected target value F of the electric push rod output force is obtained through the dynamic model of the lumbar rehabilitation flexible exoskeleton qd The expected target push rod output force is compared with the actual push rod driving force f fed back by the pull-up pressure sensor of each push rod in real time. qd For comparison, the error ΔF is input into the LESO-SMC force controller, which estimates and compensates for it based on factors such as external interference, and sends the control torque of the electric push rod to the driver of the electric push rod, thereby controlling the output force of the electric push rod and realizing the function of the lumbar rehabilitation flexible exoskeleton to provide patients with stable lumbar support assistance.

[0193] The response frequency of the force controller is much greater than that of the position controller; the adjustment of the force control to the position control is based on the error ΔF of the target support assistance and the allowable error threshold, which is divided into three specific cases: where η is the adjustment threshold and ε is the safety parking threshold;

[0194] a) When the error ΔF≤η of the target support assistance changes within a small range, the high-level force controller does not participate in the control, and the position controller independently controls the lumbar rehabilitation flexible exoskeleton to maintain follow-up;

[0195] b) When the error of the target support force η<ΔF≤ε, but changes within a certain safety range, the high-level force controller moderately adjusts the position control to make the lumbar rehabilitation flexible exoskeleton system return to normal as soon as possible, and then allows the position controller to independently control and maintain the follow-up;

[0196] c) When the error of the target support assistance exceeds the preset safety threshold, that is, ΔF>ε, the high-level force controller enables the system to stop safely as soon as possible to avoid unpredictable safety accidents and harm to the patient.

[0197] The LESO-SMC design block diagram in the patent of this invention is as follows Figure 6 shown.

[0198] Figure 7 This is the control block diagram of the four electric push rod subsystems.

[0199] Figure 8 It is a control strategy block diagram of the lumbar rehabilitation flexible exoskeleton robot.

[0200] The lumbar rehabilitation flexible exoskeleton robot of the embodiment of the present invention has two working modes:

[0201] Follow-up lumbar decompression mode and active rehabilitation training mode. The follow-up lumbar decompression mode is mainly for the conservative treatment of patients with lumbar disc herniation. In the follow-up power mode, the lumbar rehabilitation flexible exoskeleton supports the patient's upper body weight through an electric push rod to reduce the pressure on the lumbar spine, dynamically unloads the lumbar load, and detects the posture changes of the human waist according to the inertial measurement unit. It follows the movement of the human waist in real time. It can be worn in work and life without delaying work, replacing the traditional mechanical traction plus bed rest therapy.

[0202] The active rehabilitation training mode is mainly for patients who need rehabilitation training during the conservative treatment of lumbar disc herniation or in the late stage of recovery from lumbar surgery. Regular rehabilitation training within the permitted range during the two to three months of recovery from lumbar surgery can avoid rehabilitation at the expense of muscle atrophy. Patients follow virtual reality games for active rehabilitation training, and the lumbar rehabilitation flexible exoskeleton provides partial assistance to patients, but passive training that relies entirely on the exoskeleton is not possible. In the active rehabilitation training mode, the sensors on the exoskeleton can evaluate the patient's strength and movement, and can provide quantitative feedback to the rehabilitation therapist on the patient's recovery. In this mode, the reference trajectory of the control system is determined by the therapist based on the patient's actual recovery status, and the lumbar rehabilitation flexible exoskeleton will repeat specific training trajectories and prescriptions according to the training indicators set by the rehabilitation therapist.

[0203] Lumbar rehabilitation flexible exoskeleton follow-up lumbar decompression and active rehabilitation training control process Fig. 9 The patient puts on the lumbar rehabilitation flexible exoskeleton until it is appropriately tight, turns on the power switch, and the system status is initialized. The therapist measures the width a of the patient's lower chest platform (rib platform) and pelvic platform. s and b s , input the system through the host computer interface, determine the target support force F input system and adjust it to the applicable state according to the patient's feelings. Then enter the control mode selection interface and select the mode required by the patient. The difference between the follow-up lumbar decompression mode and the active rehabilitation training mode is that the posture input of the dynamic platform is detected by the IMU or the therapist presets the trajectory. After the input is determined, both modes use the force-position hybrid control algorithm based on the LESO-SMC controller to achieve motion trajectory tracking and lumbar support assistance control.

[0204] It should be understood that the specific embodiments described above are only used to explain the present invention, and are not used to limit the present invention. Obvious changes or modifications derived from the spirit of the present invention are still within the protection scope of the present invention.

Claims

1. A force / position hybrid control method for a lumbar rehabilitation flexible exoskeleton robot based on LESO-SMC, characterized in that: The exoskeleton robot comprises a plurality of lower support frames (1) located on the same horizontal plane and a plurality of upper support frames (2) located on the same horizontal plane, wherein the plurality of lower support frames (1) are connected by a lower flexible adjustment belt (3) for wearing outside the pelvis; the plurality of upper support frames (2) are connected by an upper flexible adjustment belt (4) for wearing outside the lower ribs; a lumbar curvature support assembly (5) is arranged at the back waist position between the lower flexible adjustment belt (3) and the upper flexible adjustment belt (4); four electric push rods (6) are arranged in parallel between the lower support frame (1) and the upper support frame (2); the lower end of the electric push rod (6) is connected to the lower support frame (1) through a ball joint, and the upper end of the electric push rod (6) is connected to the upper support frame (2) through a universal joint; the upper support frame (2) and the upper flexible adjustment belt (4) form a dynamic plane. The lower support frame (1) and the lower flexible adjustment belt (3) form a fixed platform; four electric push rods (6) form a 4SPU parallel mechanism with the fixed platform and the moving platform; a linear displacement sensor for real-time detection of the extension length of each electric push rod (6) is arranged on each electric push rod (6); a first inertial measurement sensor IMU1 is installed on the inner side of the lower flexible adjustment belt; a second inertial measurement sensor IMU2 is installed on the inner side of the upper flexible adjustment belt; when mounted, the two sensors correspond to each other up and down and are located at the human spine; a tension and pressure sensor is arranged at the inner side of the lower flexible adjustment belt at a position in contact with the bony protrusion of the upper edge of the pelvis, and a tension and pressure sensor is also arranged at the inner side of the upper flexible adjustment belt at a position in contact with the bony protrusion of the lower edge of the ribs; the lumbar rehabilitation flexible exoskeleton robot cooperates with the lumbar movement to form a 4SPU-SP bionic parallel mechanism; The force / position hybrid control method comprises the following steps: S1. Establish the kinematic model of the flexible exoskeleton robot for lumbar rehabilitation; {O} = {O-XYZ} is defined as the base coordinate system, which represents the coordinate system fixed on the fixed platform; and {p} = {p-xyz} is defined as the motion coordinate system, which represents the coordinates on the moving platform; l1...l4 represent the drive rods, i.e., the electric push rods (6), each of which is composed of a motion rod l Bi and piston rod mi l0 represents the spine of the body, which can be regarded as a passive rod; the ends of the lower platform are S pairs, denoted as S i (i=1,2,3,4), each end point of the upper platform is a U pair, denoted as U i (i=1,2,3,4); the auxiliary coordinate system can be defined along with the driving rod ij ,i=1...4,j=1...7, where i is the number of driving rods; The lengths of the four electric push rods (6) are the input variables of the exoskeleton robot, denoted as l i (i=1,2,3,4), the output variable of the robot is the position and posture of the moving platform, that is, the roll angle of the moving platform rotating around the X-axis of the fixed coordinate system, the pitch angle of the moving platform rotating around the Y-axis, and the deflection angle of the moving platform rotating around the Z-axis, which are denoted as γ, β, and α respectively. The position and posture parameters of the moving platform can be expressed as q=[pz,α,β,γ] T , where p z is the Z-axis coordinate of the origin p of the moving coordinate system {p-xyz} in the fixed coordinate system {O-XYZ}, and its value is the initial rod length of the virtual constraint branch SP, which is determined by the vertical dimension between the wearer's lower chest and hip circumference; the length and width of the moving platform are respectively expressed as b h and b s Indicates that the length and width of the fixed platform are distributed by a h and a s Indicates that its value is determined by the wearer's underbust and hip measurements; The endpoints S of the given platform B can be obtained i In the fixed platform coordinate system {O b =O b -X b Y b Z b The following coordinates represent: The Euler transformation method is used to represent the position and posture of the lumbar rehabilitation flexible exoskeleton bionic parallel mechanism. The position vector of the origin p of the moving coordinate system {p-xyz} relative to the fixed coordinate system {O} is set to be p = [p x ,p y ,p z ], the posture of the motion platform is expressed by the zyx-type Euler angle, then the rotation matrix of the motion platform in the fixed coordinate system {O} is: where c j = cosj, s j = sinj, j = α, β, γ Bionic parallel mechanism moving platform m each end point U i The position vector in the moving coordinate system {p-xyz} is According to formula (1-2), (1-3), we can get U i The position vector of the point in the fixed coordinate system {O} is The initial rod length of the constraint branch SP can be determined according to the initial distance between the moving platform and the fixed platform, that is, the Z-axis coordinate p of the origin p of the moving coordinate system in the fixed coordinate system {O-XYZ} z , According to l i =| O U i - O S i |. The length expressions of each driving branch rod can be obtained: This is the inverse solution of the kinematic position of the exoskeleton robot. If the attitude angle (α, β, γ) of the moving platform relative to the fixed platform is known, the length of the four telescopic rods required can be obtained; The attitude angle measured by the first inertial measurement sensor IMU1 is The attitude angle measured by the first inertial measurement sensor IMU2 is Then the relative posture of the human waist, that is, the posture angle (α, β, γ) of the bionic parallel mechanism moving platform relative to the fixed platform can be expressed as S2. Establish the dynamic model of 4SPU-SP bionic parallel mechanism; Assume G0, f0 and n0 are the gravity vector, inertia force and inertia moment of the dynamic platform of the lumbar rehabilitation flexible exoskeleton bionic parallel mechanism. The dynamic platform includes the upper body of the human body. m0 represents the mass of the dynamic platform of the bionic parallel mechanism, which can be approximated as the mass of the upper body, which is about two-thirds of the human body weight. Assume G Bi 、f Bi and n Bi is the gravity vector, inertia force and inertia moment of the swing arm of the electric push rod (6), G mi 、f mi and n mi is the gravity vector, inertia force and inertia moment of the piston rod, m Bi and m mi are the masses of the swing rod and telescopic rod respectively; In the formula, m I0, i I Bi and m I Bi are the inertia matrices of the parallel mechanism moving platform, swing rod and telescopic rod relative to the center of mass coordinate system; is the rotation matrix of the centroid coordinate system of the i-th driving branch relative to the fixed coordinate system; the inertia parameters are obtained by Solidworks software based on the model measurement; according to the principle of virtual work, the sum of the external force / torque and inertia force / torque of each component of the parallel robot and the virtual work done by the driving force is zero, so the dynamic equation of the bionic parallel mechanism can be described as In the formula, F q =[F T T T ] T =[F l1 ...F l4 ] T is the vector of driving force in 4-SPU / SP bionic parallel mechanism; V r =J m V,[v Bi ω Bi ] T =J Bi V,[v mi ω mi ] T =J mi V is put into formula (2-2) to get That is the inverse dynamics model of the 4SPU-SP bionic parallel mechanism; S3. Through forward dynamics analysis, the rigid body dynamics of the four-degree-of-freedom rehabilitation robot is described as a second-order system; the inverse dynamics model of the bionic parallel mechanism of formula (2-3) is converted into Lagrangian form and described as a second-order nonlinear differential equation, which is expressed as follows: Where q is the required drive rod length vector, M(q) is the exoskeleton system mass matrix, represents the Coriolis term and the centripetal term, G(q) represents the gravitational effect of the system's moving platform and upper body, is the unknown disturbance, F q is a vector composed of the forces generated by the four driving electric push rods (6), τ c The force vector representing the 4 degrees of freedom; and τ p Respectively represent the control torques of the force controller and the position controller on the electric push rod (6); Considering the modeling error of the bionic parallel mechanism, in and is M(q) and The estimated value of the known part, ΔM(q) and ΔC(q) are the corresponding estimation errors; so the dynamic model differential equation (2-1) can be written as in, The dynamic model (3-3) of the four-degree-of-freedom bionic parallel mechanism can be expressed as follows: Taking the first electric push rod (6) subsystem of the lumbar rehabilitation flexible exoskeleton bionic parallel mechanism as an example, its dynamic differential equation can be expressed as Can be written as in Assumptions The derivative of exists, and The derivative of is bounded; let q=y1=x1, The total disturbance is the expanded state variable Its time derivative is Then (3-7) can be written in the state space as Based on (3-9), the linear extended state observer LESO can establish the following formula: Where p1, p2 and p3 are LESO parameters. By properly choosing the parameters in (3-10), the output of LESO is The states x1, x2, and x3 can be tracked; Taking the first degree of freedom in the dynamic model (3-5) as an example, define the sliding plane Where c1>0 is a known parameter, is the output of the linear extended state observer LESO, y 1d is a bounded expected output, then the synovial controller is: Considering the stability of the sliding mode controller SMC, the following Lyapunov function is obtained: According to the definition of sliding surface in (3-11), the differential of s can be given as: in is the LESO error generated by tracking the actual output, is the estimated value of y1; review From the definition of , we can get the time derivative of V as The above formula can be written as By definition The following formula can be obtained It is not difficult to find If k1>1 / 2, so It can be seen that the tracking error of the closed-loop system converges to the bounded zero neighborhood, and the convergence speed of the system is determined by the sliding mode parameter k1; S4. Obtain the waist movement posture through waist movement intention perception based on inertial measurement sensor, and track and detect the change of waist posture in real time to realize the follow-up control of lumbar rehabilitation flexible exoskeleton, so as to achieve position control; The inverse kinematics model (1-8) is converted into a mathematical model of the control system, specifically: Where, i = 1, 2, 3, 4; f i1 、f i2 、f i3 It is a function related to the coordinate axes X, Y, and Z, and its expression is about the variables (α, β, γ, p z )’s trigonometric functions; When the human waist motion posture is given by the inertial measurement sensor, the length of the branch electric push rod (6) driven by the lumbar rehabilitation flexible exoskeleton bionic parallel mechanism can be uniquely solved; when the lumbar rehabilitation flexible exoskeleton is in follow-up decompression and active rehabilitation training, the inertial measurement sensor tracks and detects the changes in the waist posture Δα, Δβ, Δγ in real time, and substitutes them into formula (4-1) to solve the expected target length of each electric push rod (6): The displacement sensor integrated in the electric push rod (6) provides real-time feedback of the actual rod length l i , the error ΔL between the actual length of the electric push rod (6) and the expected rod length i = l i -L i , as the input of the LESO-SMC position controller, is sent to the linear extended state observation sliding mode controller for error estimation and compensation, and the output position control torque τ of the electric push rod (6) is p A driver is provided to the electric push rod (6), and the driver outputs a corresponding voltage to control the movement of the push rod, thereby realizing the follow-up control of the lumbar vertebra rehabilitation flexible exoskeleton; S5. The posture angle of the dynamic platform and the target support force F required by the patient are obtained by sensing the waist movement intention based on the inertial measurement sensor as system input, and then substituted into the inverse dynamics model (2-3) to solve the target value F of the electric push rod output force qd As the output of the system, it is controlled by the force controller to achieve force control; The inverse dynamics model of the lumbar spine rehabilitation flexible exoskeleton is written in Lagrangian form: Where, q = [α β γ F], represents the input of the lumbar rehabilitation flexible exoskeleton system; D(q) is the inertia matrix of the robot system, which is the driving force required to achieve the corresponding acceleration of the mechanism; is the driving force required by the robot system to overcome the centrifugal force and Coriolis force vector velocity factors; G(q) is the driving force required by the robot system to overcome the gravity factor of the mechanism; τ is the external interference force acting on the lumbar rehabilitation flexible exoskeleton system; (J T ) + is the pseudo-inverse matrix of the Jacobian matrix J; The expected target value F of the electric push rod output force is obtained through the dynamic model of the lumbar rehabilitation flexible exoskeleton qd The expected target push rod output force is compared with the actual push rod driving force f fed back by the pull-up pressure sensor of each push rod in real time. qd For comparison, the error ΔF is input into the LESO-SMC force controller, which estimates and compensates for external interference factors, and sends the control torque of the electric push rod to the driver of the electric push rod, thereby controlling the output force of the electric push rod and realizing the function of the lumbar rehabilitation flexible exoskeleton to provide patients with stable lumbar support assistance.

2. The force / position hybrid control method of a lumbar rehabilitation flexible exoskeleton robot based on LESO-SMC according to claim 1 is characterized in that: The response frequency of the force controller is much greater than that of the position controller; the adjustment of the force control to the position control is based on the error ΔF of the target support assistance and the allowable error threshold, which is divided into three specific cases: where η is the adjustment threshold and ε is the safety parking threshold; a) When the error ΔF≤η of the target support assistance changes within a small range, the high-level force controller does not participate in the control, and the position controller independently controls the lumbar rehabilitation flexible exoskeleton to maintain follow-up; b) When the error of the target support force η<ΔF≤ε, but changes within a certain safety range, the high-level force controller moderately adjusts the position control to make the lumbar rehabilitation flexible exoskeleton system return to normal as soon as possible, and then allows the position controller to independently control and maintain the follow-up; c) When the error of the target support assistance exceeds the preset safety threshold, that is, ΔF>ε, the high-level force controller enables the system to stop safely as soon as possible to avoid unpredictable safety accidents and harm to the patient.

Images