Control method of three-dimensional limb mirror rehabilitation robot and rehabilitation robot

Abstract

The application provides a control method and a rehabilitation robot of a three-dimensional limb mirror rehabilitation robot, which integrates calculation and smoothing of a healthy side upper limb joint angle into one step of an unscented Kalman filter, has higher algorithm integration degree, causes smaller delay, and calculates the angular velocity of each joint of the robot based on a model predictive control algorithm, so that the algorithm has a certain predictability due to the existence of the prediction model, thereby significantly reducing algorithm delay and the angle error of each joint of the healthy side limb and the hemiplegic side limb driven by the robot caused by the delay.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of rehabilitation robots, in particular to a control method of a three-dimensional limb mirror rehabilitation robot and the rehabilitation robot. BACKGROUND

[0002] The existing three-dimensional upper limb mirror rehabilitation robot reads the three-dimensional coordinates of the joint centers of the healthy side upper limb of the human body from a depth vision sensor, then calculates the angles of each joint of the healthy side upper limb by using geometric relationships, and sends the angles of each joint of the healthy side upper limb to the robot after filtering and smoothing, so that the robot drives each joint of the hemiplegic side upper limb to move to a position mirroring the healthy side arm.

[0003] Due to the delay caused by the control algorithm and communication, there is a large angle error between the actively moving healthy side limb and the hemiplegic side limb driven by the robot. SUMMARY

[0004] The problem solved by the present application is that there is a large angle error between the actively moving healthy side limb and the hemiplegic side limb driven by the robot during mirror rehabilitation training.

[0005] To solve the above problems, the present application provides a control method of a three-dimensional limb mirror rehabilitation robot, which comprises: acquiring three-dimensional coordinates of the centers of a plurality of joints of a healthy side limb; calculating and smoothing the joint angles of the healthy side limb based on the three-dimensional coordinates and a unscented Kalman filter algorithm to obtain the angles and angular velocities of a plurality of joints; calculating the angular velocities of the corresponding joints of an exoskeleton based on the angles and angular velocities of a plurality of joints and a model predictive control algorithm; and controlling the movement of the exoskeleton according to the angular velocities of the corresponding joints of the exoskeleton to drive the limb to move to a position mirroring the healthy side limb.

[0006] Optionally, the center of the joint comprises at least one of the following: the center of the shoulder joint of the healthy side upper limb, the center of the elbow joint of the healthy side upper limb, and the center of the wrist joint of the healthy side upper limb.

[0007] Optionally, the calculation and smoothing of the joint angles of the healthy side limb based on the three-dimensional coordinates and the unscented Kalman filter algorithm to obtain the angles and angular velocities of a plurality of joints comprises: inputting the three-dimensional coordinates of the center of the shoulder joint of the healthy side upper limb, the center of the elbow joint of the healthy side upper limb, and the center of the wrist joint of the healthy side upper limb at the current time into the unscented Kalman filter algorithm to calculate and smooth the joint angles of the healthy side upper limb, and outputting the abduction / adduction angle of the shoulder joint of the healthy side upper limb, the flexion / extension angle of the shoulder joint, the flexion / extension angle of the elbow joint, and the abduction / adduction angular velocity of the shoulder joint of the healthy side upper limb, the flexion / extension angular velocity of the shoulder joint, and the flexion / extension angular velocity of the elbow joint.

[0008] Optionally, the state variables of the healthy side upper limb system are as follows:

[0009]

[0010] wherein q1(k), q2(k), q3(k) represent the shoulder joint abduction / adduction angle, the shoulder joint flexion / extension angle, the elbow joint flexion / extension angle of the healthy upper limb at the current time k respectively; dq1(k), dq2(k), dq3(k) represent the shoulder joint abduction / adduction angle velocity, the shoulder joint flexion / extension angle velocity, the elbow joint flexion / extension angle velocity respectively; x1(k), y1(k), z1(k) represent the three-dimensional coordinates of the shoulder joint center S of the healthy upper limb respectively; dx1(k), dy1(k), dz1(k) represent the three-dimensional translation velocities of the shoulder joint center S of the healthy upper limb respectively; L1(k), L2(k) represent the length of the upper arm and the length of the lower arm of the healthy upper limb at the time k respectively;

[0011] Assuming that each joint of the healthy upper limb moves at a constant speed and is subject to random disturbance, the discrete state equation of the healthy upper limb system is:

[0012]

[0013] wherein k-1 represents the previous time; T represents the control period; W(k) = [w1(k), w2(k), w3(k), w4(k), w5(k), w6(k)] T represent the random disturbance suffered by the system, which are subject to normal distribution with zero mean;

[0014] The system observation equation is obtained based on forward kinematics

[0015]

[0016] wherein Z(k) = [Z1(k), Z2(k), Z3(k), Z4(k), Z5(k), Z6(k), Z7(k), Z8(k), Z9(k)] T represent the system observation variables;

[0017] V(k) = [v1(k), v2(k), v3(k), v4(k), v5(k), v6(k), v7(k), v8(k), v9(k)] T represent the system measurement errors, which are subject to normal distribution with zero mean;

[0018] The actual observation of the system is composed of the three-dimensional coordinates [x1(k), y1(k), z1(k)] of the shoulder joint center of the healthy upper limb, the three-dimensional coordinates [x2(k), y2(k), z2(k)] of the elbow joint center of the healthy upper limb, and the three-dimensional coordinates [x3(k), y3(k), z3(k)] of the wrist joint center of the healthy upper limb;

[0019] Set the initial value X(0) of the state variable, the variance of the random disturbance W(k) and the variance of the measurement error V(k), and solve the discrete state equation, the system observation equation and the actual observation [x1(k), y1(k), z1(k), x2(k), y2(k), z2(k), x3(k), y3(k), z3(k)] T Substitute into the preset unscented Kalman algorithm to solve the filtered healthy upper limb shoulder joint abduction / adduction angle q1(k), shoulder joint flexion / extension angle q2(k), elbow joint flexion / extension angle q3(k) and healthy upper limb shoulder joint abduction / adduction angle velocity dq1(k), shoulder joint flexion / extension angle velocity dq2(k), elbow joint flexion / extension angle velocity dq3(k).

[0020] Optionally, based on the angles, angular velocities of the plurality of joints and the model predictive control algorithm, the angular velocity of the corresponding joint of the exoskeleton is calculated, including: inputting the healthy upper limb shoulder joint abduction / adduction angle, the shoulder joint flexion / extension angle, the elbow joint flexion / extension angle and the healthy upper limb shoulder joint abduction / adduction angle velocity, the shoulder joint flexion / extension angle velocity and the elbow joint flexion / extension angle velocity at the current time into the model predictive control algorithm, and outputting the shoulder joint abduction / adduction angle velocity, the shoulder joint flexion / extension angle velocity and the elbow joint flexion / extension angle velocity of the exoskeleton.

[0021] Optionally, the state variable X(k) of the exoskeleton system is [θ1(k), θ2(k), θ3(k)] T ; wherein θ1(k), θ2(k), θ3(k) represent the shoulder joint abduction / adduction angle, the shoulder joint flexion / extension angle and the elbow joint flexion / extension angle of the exoskeleton at time k respectively;

[0022] The prediction model of the exoskeleton system is as follows:

[0023] X(k+1)=A*X(k)+B*U(k)

[0024] Wherein is a 3*3 unit matrix; T represents a control period; U(k)=[dθ1(k), dθ2(k), dθ3(k)] T represents the angular velocity of each joint of the exoskeleton input by the system;

[0025] The model predictive control is equivalent to solving the following quadratic optimization equation:

[0026]

[0027] Wherein, p is a preset prediction step; X(k+i) ref= [q1(k) + i*dq1(k), q2(k) + i*dq2(k), q3(k) + i*dq3(k)] T , i = 1...p is the reference trajectory composed of the filtered contralateral upper limb shoulder abduction / adduction angle q1(k), shoulder flexion / extension angle q2(k), elbow flexion / extension angle q3(k) and the contralateral upper limb shoulder abduction / adduction angle velocity dq1(k), shoulder flexion / extension angle velocity dq2(k), elbow flexion / extension angle velocity dq3(k); Q i ∈ R p×p and R i ∈ R p×p is a preset diagonal weight matrix; U(k+i) = [dθ1(k+i), dθ2(k+i), dθ3(k+i)] T The constraint condition needs to be met where dθ1 min , dθ1 max respectively represent the minimum and maximum angle velocities allowed for the exoskeleton shoulder abduction / adduction, dθ2 min , dθ2 max respectively represent the minimum and maximum angle velocities allowed for the exoskeleton shoulder flexion / extension, dθ3 min , dθ3 max respectively represent the minimum and maximum angle velocities allowed for the exoskeleton elbow flexion / extension;

[0028] A prediction step p is set, a weight matrix Q i , R i and a constraint condition are set, and the quadratic optimization equation is substituted into the existing model predictive control algorithm for solving, so as to solve the angle velocities U(k+1) of each joint of the exoskeleton at the next time point, including the exoskeleton shoulder abduction / adduction angle velocity dθ1(k+1), the exoskeleton shoulder flexion / extension angle velocity dθ2(k+1) and the exoskeleton elbow flexion / extension angle velocity dθ3(k+1).

[0029] Optionally, the exoskeleton is controlled to move according to the angle velocities of the corresponding joints of the exoskeleton, so that the exoskeleton drives the limb to move to a position mirroring the contralateral limb, including: the exoskeleton is controlled to move according to the periodic synchronous velocity mode and the angle velocities of the corresponding joints of the exoskeleton, so that the exoskeleton drives the limb to move to a position mirroring the contralateral limb.

[0030] The application provides a rehabilitation robot, comprising a visual sensor, an industrial control host, an exoskeleton and an exoskeleton controller; the visual sensor is used for acquiring three-dimensional coordinates of centers of joints of a patient's limb; the exoskeleton is used for applying an auxiliary torque to a limb of a side to be rehabilitated of the patient to drive the limb of the side to be rehabilitated to move; the industrial control host is used for executing the control method of the three-dimensional limb mirror rehabilitation robot; and the exoskeleton controller is used for controlling the exoskeleton to move according to angular velocities of joints of the exoskeleton, so that the exoskeleton drives the limb to move to a position mirroring the healthy side limb.

[0031] Optionally, the application further comprises a scene display module; the scene display module is used for displaying a mirror rehabilitation training scene.

[0032] Optionally, the exoskeleton controller comprises a periodic synchronization speed mode, which is used for adjusting the angular velocities of the joints of the exoskeleton in real time.

[0033] The application provides a control method of a three-dimensional limb mirror rehabilitation robot and the rehabilitation robot, which integrates calculation and smoothing of angular degrees of a healthy side upper limb into one step of a Kalman filter, has higher algorithm integration, causes smaller delay, and calculates angular velocities of joints of the robot based on a model prediction control algorithm; the algorithm has a certain predictability due to the prediction model, so that the algorithm delay and the angular degree error between the healthy side limb and the hemiplegic side limb driven by the robot caused by the delay can be significantly reduced. BRIEF DESCRIPTION OF DRAWINGS

[0034] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or the prior art description. Obviously, the drawings in the following description only are the embodiments of the present application, and for those skilled in the art, other drawings can be obtained without creative labor based on the provided drawings.

[0035] Figure 1 It is a structure schematic diagram of a three-dimensional upper limb mirror rehabilitation robot in the embodiment of the present application;

[0036] Figure 2 It is a schematic flow chart of a control method of a three-dimensional limb mirror rehabilitation robot in the embodiment of the present application;

[0037] Figure 3 It is a human skeleton model schematic diagram acquired by a visual sensor in the embodiment of the present application;

[0038] Figure 4 It is a structure schematic diagram of an upper limb exoskeleton in the embodiment of the present application;

[0039] Figure 5A model prediction control effect comparison chart of the embodiment of the present application and prior art.

[0040] Explanation of reference signs:

[0041] 10 - display device; 20 - depth vision sensor; 30 - industrial control host; 40 - upper limb exoskeleton; 50 - exoskeleton controller; 301 - healthy side upper limb shoulder joint center; 302 - healthy side upper limb elbow joint center; 303 - healthy side upper limb wrist joint center; 401 - shoulder joint abduction / adduction degree of freedom; 402 - shoulder joint flexion / extension degree of freedom; 403 - elbow joint flexion / extension degree of freedom; 404 - large arm length adjustment module; 405 - small arm length adjustment module. DETAILED DESCRIPTION

[0042] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the specific embodiments of the present application will be described in detail below with reference to the drawings. It should be understood that the specific embodiments described herein are only used to explain the present application and do not limit the present application.

[0043] Stroke has five characteristics of high incidence, high mortality, high disability rate, high recurrence rate and high economic burden, which makes the patient's life dependent degree high and the quality of life significantly decreased. The loss of upper limb and hand function is the culprit of poor life independence. In addition to secondary prevention after stroke, rehabilitation therapy has always been an important means for the recovery of dysfunction after stroke.

[0044] The traditional rehabilitation therapy method has limited effect on the treatment of patients with post-stroke residual dysfunction. Mirror therapy, as a low-cost, safe and easy treatment method, has been widely used in the rehabilitation therapy of many diseases. Mirror therapy induces motor cortex neural network reorganization by visual feedback to induce the interaction between the undamaged half brain and the damaged half brain, thereby promoting the plasticity of cerebral cortex nerves, achieving the purpose of eliminating abnormal sensation or restoring motor function. This method has significant effect and has been widely used in clinical practice.

[0045] Traditional mirror therapy relies on mirror reflection. The mirror is placed between the affected limb and the healthy limb, the affected limb is placed behind the mirror, and the healthy limb is placed in front of the mirror. The patient tries to make the same movement as the healthy limb as instructed by the doctor. When the patient moves the bilateral limbs at the same time, the movement of the healthy limb in the mirror will be regarded as the movement of the affected limb. This method is limited by the device, and the patient's training range is restricted. Moreover, the patient must be guided by the doctor; the mirror rehabilitation training action is determined by the therapist subjectively, the form is single, and it is difficult to mobilize the enthusiasm of the patient, resulting in low rehabilitation efficiency.

[0046] The existing upper limb mirror rehabilitation robots are mainly divided into two types: end-effector type and exoskeleton type upper limb rehabilitation robots. The current research on upper limb mirror rehabilitation robots is still in the laboratory stage, and all of them belong to end-effector type robots.

[0047] However, the real upper limb rehabilitation training is complex, and for the training of a certain part of the upper limb, the end-effector type upper limb rehabilitation robot cannot meet the requirements, and unnecessary rehabilitation movements may be introduced, or the required rehabilitation movements cannot be fully implemented. Therefore, the other type of upper limb mirror rehabilitation robot, the exoskeleton type upper limb rehabilitation robot, has gradually become a research hotspot of upper limb mirror rehabilitation robots. The existing mirror rehabilitation robot prototypes are too complex or too high in cost, which makes it difficult to ensure safety and commercialization. With the mirror therapy being proved to be more and more effective, the clinical demand for mirror rehabilitation robots will be greater and greater, and the embodiments of the present application can provide a low-cost and easy-to-operate rehabilitation robot.

[0048] An existing somatosensory control method of an upper limb rehabilitation robot adopts Kinect to collect upper limb movement data, processes the movement data through an upper computer to obtain a control signal, and transmits the control signal to a servo driver through a lower computer to drive an exoskeleton wearable mechanical arm to drive a limb for rehabilitation training. The rehabilitation training strategy is: bilateral limb mirror synchronous rehabilitation training under mirror somatosensory control; master-slave type rehabilitation training under synchronous somatosensory control; and semi-active rehabilitation training. The scheme adopts somatosensory control of limb movement, and the control mode is more intuitive, natural and simple, can perform multi-joint real-time linkage control, and is convenient for training of complex movements; the somatosensory master-slave real-time control mode is convenient for a physiotherapist to adjust a rehabilitation training scheme in real time; and a patient can drive a limb to perform healthy-limb-limb bilateral coordination synchronous mirror movement through movement of a healthy limb, which is more helpful for promoting repair of brain neurons and improving the treatment effect of mirror therapy on the entire affected limb.

[0049] An existing somatosensory control upper limb exoskeleton mirror rehabilitation robot includes a Kinect sensor, a master control computer, a 3-degree-of-freedom exoskeleton wearable mechanical arm, and a somatosensory control system of the mechanical arm. The Kinect sensor is responsible for collecting and processing joint angle information of a healthy upper limb of a human body, and the exoskeleton wearable mechanical arm includes an elbow flexion / extension structure, a shoulder flexion / extension structure, and a shoulder abduction / adduction structure. The scheme has the characteristics of somatosensory direct control, and does not need to wear a signal collection device, which is convenient to operate. A patient can drive a limb to perform a new type of bilateral mirror synchronous rehabilitation training under the assistance of the robot through movement of a healthy limb, or can perform master-slave type rehabilitation training through movement of a limb on the same side as the patient's affected limb by a physiotherapist.

[0050] The existing human upper limb rehabilitation training and motion assisting system based on a Kinect sensor includes a human motion recognition system, an exoskeleton device and a control box; the human motion recognition system includes a sensor module, a data collection module, a data cache module, a microprocessor and a digital-analog converter connected in sequence; the exoskeleton device provides three-degree-of-freedom auxiliary motion for the double arms, and includes a motor and an encoder; the control box includes an I / O module and a processor chip connected with each other, and the processor chip is connected with a remote control computer. The system uses the sensor module to realize human-computer interaction, and uses the exoskeleton device to realize the auxiliary and rehabilitation functions of the upper limb motion. For the upper limb dysfunction, the motion information of the upper limb of the patient is detected through the sensor module, the motion trend of the upper limb is determined by identifying the angle of the human skeleton and the upper limb, and the three degrees of freedom of the shoulder joint flexion / extension, abduction / adduction and elbow joint flexion / extension can be assisted.

[0051] The three-dimensional upper limb mirror rehabilitation robot reads the three-dimensional coordinates of the joint centers of the healthy upper limb of the human body from a depth vision sensor (such as Microsoft Kinect, Intel RealSense, etc.), then calculates the angles of the joints of the healthy upper limb by using geometric relationships (cosine theorem, etc.), and sends the angles of the joints of the healthy upper limb to the robot after filtering and smoothing, so that the joints of the hemiplegic upper limb are moved to the positions of the healthy upper limb mirror by the robot. Due to the delay caused by the control algorithm and communication, there is a large angle error between the actively moving healthy upper limb and the hemiplegic upper limb moved by the robot. Taking Table 1 as an example, the angle error between the healthy upper limb and the hemiplegic upper limb reaches 12.3 degrees at most, and the time delay reaches 0.8 seconds at most.

[0052]

[0053]

[0054] Table 1

[0055] In view of the above problems, the present application provides a rehabilitation robot, which aims to reduce the angle error between the actively moving healthy limb and the hemiplegic limb moved by the robot, and improve the accuracy of limb rehabilitation training.

[0056] In the embodiment, the upper limb rehabilitation robot is taken as an example for description, and the present application can also be applied to other limb rehabilitation robots. Figure 1 A three-dimensional upper limb mirror rehabilitation robot structure schematic diagram is shown.

[0057] The three-dimensional upper limb mirror rehabilitation robot comprises a display end and an exoskeleton end. The display end can comprise a display device 10, a depth vision sensor 20 and an industrial control host 30. The display device 10 can display a rehabilitation training scene, the vision sensor 20 can be fixedly installed on the display end to capture three-dimensional coordinates of joint centers of a healthy side arm, and the industrial control host 30 can run rehabilitation training software. The generation, update and all algorithms of the rehabilitation training scene are realized by the rehabilitation training software. The exoskeleton end comprises an upper limb exoskeleton 40 and an exoskeleton controller 50. The upper limb exoskeleton 40 can provide auxiliary force / torque to drive the hemiplegic side arm of the patient to move, and the exoskeleton controller 50 can adjust the angular velocity of each joint of the upper limb exoskeleton.

[0058] The upper limb exoskeleton has left and right hand switching function, arm length adjustment function, and at least contains the following three degrees of freedom: shoulder joint abduction / adduction degree of freedom, forward bending / rear stretching degree of freedom, and elbow joint flexion / extension degree of freedom.

[0059] The rehabilitation training software generates a rehabilitation training scene, and the rehabilitation training scene is composed of a series of tasks. The scene task has a cognitive training function, and the cognitive ability of the patient is trained while the limb is trained, so as to more effectively promote the plasticity change and functional reorganization of the brain, and promote the functional recovery of the damaged area.

[0060] Exemplarily, the working process of the above-mentioned upper limb mirror rehabilitation robot is as follows:

[0061] 1. The patient sits in the middle position behind the vision sensor, the upper arm and forearm of the hemiplegic side upper limb of the patient are fixed on the exoskeleton mechanical arm through the adjustable Velcro respectively, and the rehabilitation training software is started;

[0062] 2. The rehabilitation training software generates a rehabilitation training scene, and the rehabilitation training scene is composed of a series of tasks. The task requires the patient to move the healthy side arm to the target position autonomously;

[0063] 3. The three-dimensional coordinates of the joint centers of the healthy side upper limb are captured by the depth vision sensor, the joint angles of the healthy side upper limb are calculated and smoothed based on the Unscented Kalman Filter (UKF) algorithm, and the angular velocity of each joint of the exoskeleton is calculated based on the Model Predictive Control (MPC) algorithm;

[0064] 4. Each joint of the exoskeleton drives the hemiplegic side arm of the patient to move to the position mirroring the healthy side arm under the control of the exoskeleton controller according to the angular velocity;

[0065] 5. When the healthy side and hemiplegic side arms of the patient both reach the target position of the task, it is considered that the current task is completed. After completing the current task, the next task is sequentially entered until all tasks are completed or the training time is ended.

[0066] The embodiment of the present application provides a control method of a three-dimensional limb mirror rehabilitation robot, which can reduce the angle error of each joint of the healthy upper limb in active movement and the hemiplegic upper limb driven by the robot, and improve the accuracy of upper limb rehabilitation training.

[0067] Figure 2 is a schematic flow chart of a control method of a three-dimensional limb mirror rehabilitation robot in the embodiment of the present application, and the method comprises the following steps:

[0068] S202, three-dimensional coordinates of centers of a plurality of joints of a healthy limb are acquired.

[0069] Taking the healthy limb as an upper limb, the three-dimensional coordinates of the centers of three joints (the center of the shoulder joint, the center of the elbow joint and the center of the wrist joint) of the healthy upper limb can be captured by a depth vision sensor. For the upper limb, the embodiment only needs to capture the three-dimensional coordinates of the centers of three joints of the human body, and other existing methods need to capture more joint centers, and the present scheme has smaller calculation amount.

[0070] S204, the joint angles of the healthy limb are calculated and smoothed based on the three-dimensional coordinates and the unscented Kalman filtering algorithm, and the angles and angular velocities of a plurality of joints are obtained.

[0071] Taking the center of the joint as an example, the center of the shoulder joint of the healthy upper limb, the center of the elbow joint of the healthy upper limb and the center of the wrist joint of the healthy upper limb are included. Alternatively, the steps of calculating and smoothing the joint angles of the healthy upper limb based on the unscented Kalman filtering algorithm are as follows:

[0072] The three-dimensional coordinates of the center of the shoulder joint of the healthy upper limb, the center of the elbow joint of the healthy upper limb and the center of the wrist joint of the healthy upper limb at the current time are input into the unscented Kalman filtering algorithm to calculate and smooth the joint angles of the healthy upper limb, and the abduction / adduction angle of the shoulder joint of the healthy upper limb, the forward / backward angle of the shoulder joint, the flexion / extension angle of the elbow joint and the abduction / adduction angular velocity of the shoulder joint of the healthy upper limb, the forward / backward angular velocity of the shoulder joint and the flexion / extension angular velocity of the elbow joint are output.

[0073] In the embodiment, the calculation and smoothing of the joint angles of the healthy upper limb are integrated into one step of the unscented Kalman filtering. Other existing methods first calculate the joint angles of the healthy upper limb based on geometric relationships, and then use a filtering algorithm for smoothing, which needs to be performed in two steps. The present scheme integrates the calculation and smoothing of the joint angles of the healthy upper limb into one step by using the unscented Kalman filtering to process the nonlinear observation problem, so that the algorithm integration degree is higher and the delay caused is smaller.

[0074] S206, the angular velocities of the corresponding joints of the exoskeleton are calculated based on the angles and angular velocities of the plurality of joints and the model predictive control algorithm.

[0075] Optionally, the shoulder joint abduction / adduction angle, the shoulder joint flexion / extension angle, the elbow joint flexion / extension angle of the upper limb on the healthy side at the current time, and the shoulder joint abduction / adduction angular velocity, the shoulder joint flexion / extension angular velocity, the elbow joint flexion / extension angular velocity of the upper limb on the healthy side are input into the model predictive control algorithm, and the shoulder joint abduction / adduction angular velocity, the shoulder joint flexion / extension angular velocity, and the elbow joint flexion / extension angular velocity of the exoskeleton are output.

[0076] In the embodiment, the angular velocity of each joint of the robot exoskeleton is calculated based on the model predictive control algorithm. The algorithm has a certain predictability due to the existence of the prediction model, and can significantly reduce the algorithm delay and the angle error caused by the delay.

[0077] In S208, the exoskeleton is controlled to move according to the angular velocity of the corresponding joint of the exoskeleton, so that the exoskeleton drives the limb to move to the position mirroring the healthy side limb.

[0078] Optionally, the exoskeleton is controlled to move based on the cyclic synchronous velocity mode (CSV) and the angular velocity of the corresponding joint of the exoskeleton, so that the exoskeleton drives the limb to move to the position mirroring the healthy side limb. The angular velocity of each joint of the exoskeleton can be adjusted in real time based on the cyclic synchronous velocity mode.

[0079] When the arms of the patient on the healthy side and the hemiplegic side both reach the task target position, the current task is considered to be completed, and the next task is entered in sequence after the current task is completed, until all tasks are completed or the training time ends.

[0080] The control method of the three-dimensional limb mirroring rehabilitation robot provided in the embodiment integrates the calculation and smoothing of the joint angle of the upper limb on the healthy side into one step of the unscented Kalman filtering, has higher algorithm integration, causes smaller delay, and calculates the angular velocity of each joint of the robot based on the model predictive control algorithm. The algorithm has a certain predictability due to the existence of the prediction model, and can significantly reduce the algorithm delay and the angle error of each joint of the hemiplegic limb driven by the robot.

[0081] Figure 3 FIG. 1 is a schematic diagram of a human skeleton model acquired by a visual sensor in the embodiment. The human skeleton model at least includes a shoulder joint center 301, an elbow joint center 302, and a wrist joint center 303 of the upper limb on the healthy side.

[0082] After the three-dimensional coordinates of the joint centers of the upper limb on the healthy side are acquired by the visual sensor, the smoothed joint angle of the upper limb on the healthy side can be obtained based on the unscented Kalman filtering algorithm. The three-dimensional coordinates [x1(k), y1(k), z1(k)] of the shoulder joint center 301 of the upper limb on the healthy side at the current time k are input into the unscented Kalman filtering algorithm. T, three-dimensional coordinates of the elbow joint center 302 [x2(k), y2(k), z2(k)] T , three-dimensional coordinates of the wrist joint center 303 [x3(k), y3(k), z3(k)] T , output the healthy side upper limb shoulder joint abduction / adduction angle q1(k), shoulder joint flexion / extension angle q2(k), elbow joint flexion / extension angle q3(k), and the healthy side upper limb shoulder joint abduction / adduction angle velocity dq1(k), shoulder joint flexion / extension angle velocity dq2(k), elbow joint flexion / extension angle velocity dq3(k).

[0083] Exemplarily, the algorithm is as follows:

[0084] State variables of the healthy side upper limb system:

[0085]

[0086] wherein q1(k), q2(k), q3(k) respectively represent the healthy side arm shoulder joint abduction / adduction angle, shoulder joint flexion / extension angle and elbow joint flexion / extension angle at the current time k; dq1(k), dq2(k), dq3(k) respectively represent the healthy side arm shoulder joint abduction / adduction angle velocity, shoulder joint flexion / extension angle velocity and elbow joint flexion / extension angle velocity; x1(k), y1(k), z1(k) respectively represent the three-dimensional coordinates of the healthy side arm shoulder joint center S; dx1(k), dy1(k), dz1(k) respectively represent the three-dimensional translation velocity of the healthy side arm shoulder joint center S; L1(k), L2(k) respectively represent the lengths of the large arm and the small arm of the healthy side arm at time k.

[0087] Assuming that each joint of the healthy side upper limb moves at a constant speed and is subject to random disturbance, the system discrete state equation is

[0088]

[0089] wherein k-1 represents the last time; T represents the control period (for example, T = 1 / 30 seconds); W(k) = [w1(k), w2(k), w3(k), w4(k), w5(k), w6(k)] represents the random disturbance suffered by the system, which is subject to normal distribution with zero mean. Since the lengths of the large arm and the small arm should remain unchanged during the movement, the terms corresponding to L1 and L2 in the state equation are not affected by the random disturbance.

[0090] Based on forward kinematics, the system observation equation is obtained:

[0091]

[0092] wherein, Z(k) = [Z1(k), Z2(k), Z3(k), Z4(k), Z5(k), Z6(k), Z7(k), Z8(k), Z9(k)] T represents system observation variables;

[0093] V(k) = [v1(k), v2(k), v3(k), v4(k), v5(k), v6(k), v7(k), v8(k), v9(k)] T represents system measurement errors, all subject to normal distribution with zero mean.

[0094] The actual observation of the system is composed of the three-dimensional coordinates [x1(k), y1(k), z1(k)] of the center of the shoulder joint of the healthy upper limb, the three-dimensional coordinates [x2(k), y2(k), z2(k)] of the center of the elbow joint of the healthy upper limb, and the three-dimensional coordinates [x3(k), y3(k), z3(k)] of the center of the wrist joint of the healthy upper limb.

[0095] The initial value X(0) of the state variable and the variance of the random disturbance W(k) and the variance of the measurement error V(k) are set, and the system state equation, the observation equation and the actual observation [x1(k), y1(k), z1(k), x2(k), y2(k), z2(k), x3(k), y3(k), z3(k)] T are substituted into the existing unscented Kalman filter algorithm library (such as the unscentedKalmanFilter function in Matlab) for solving, so that the filtered shoulder abduction / adduction angle q1(k) of the healthy upper limb, the shoulder flexion / extension angle q2(k), the elbow flexion / extension angle q3(k), the shoulder abduction / adduction angle velocity dq1(k) of the healthy upper limb, the shoulder flexion / extension angle velocity dq2(k), and the elbow flexion / extension angle velocity dq3(k) are solved.

[0096] Since the observation equation contains a large number of nonlinear functions such as sin and cos, the ordinary Kalman filter algorithm cannot handle it, so other methods first calculate the joint angles of the healthy upper limb based on geometric relationships, and then use the ordinary Kalman filter algorithm for smoothing, which needs to be performed in two steps. The present embodiment utilizes the nonlinear processing capability of the unscented Kalman filter to integrate the calculation and smoothing of the joint angles of the healthy upper limb into one step, which has higher algorithm integration and causes less delay.

[0097] Figure 4An upper limb exoskeleton structure diagram is shown. The upper limb exoskeleton is composed of a shoulder joint abduction / adduction degree of freedom 401, a shoulder joint flexion / extension degree of freedom 402, an elbow joint flexion / extension degree of freedom 403, a large arm length adjustment module 404, and a small arm length adjustment module 405. Each joint degree of freedom (401-403) is provided with a driving motor to generate a torque to assist the patient in moving the corresponding joint of the hemiplegic side arm. The large arm length adjustment module 404 can adjust the length of the large arm, and the small arm length adjustment module 405 can adjust the length of the small arm to adapt to different patients.

[0098] The model predictive control algorithm is used to calculate the angular velocity of each joint of the exoskeleton, and the filtered shoulder joint abduction / adduction angle q1(k) of the healthy side upper limb at the current time k, the shoulder joint flexion / extension angle q2(k), the elbow joint flexion / extension angle q3(k), and the shoulder joint abduction / adduction angular velocity dq1(k) of the healthy side upper limb, the shoulder joint flexion / extension angular velocity dq2(k), and the elbow joint flexion / extension angular velocity dq3(k) are input, and the output is the shoulder joint abduction / adduction angular velocity dθ1, the shoulder joint flexion / extension angular velocity dθ2, and the elbow joint flexion / extension angular velocity dθ3. The specific algorithm is as follows:

[0099] The state variable X(k) of the exoskeleton system is [θ1(k), θ2(k), θ3(k)] T where θ1(k), θ2(k), and θ3(k) represent the shoulder joint abduction / adduction angle, the shoulder joint flexion / extension angle, and the elbow joint flexion / extension angle of the exoskeleton at time k, respectively, which can be measured by sensors (encoders, etc.) of each joint of the exoskeleton. Since the healthy side arm is the initiator of movement, the arm on the exoskeleton side only needs to track the movement trajectory of the healthy side arm, and the state variable of the exoskeleton is less.

[0100] Exoskeleton system prediction model

[0101] X(k+1) = A*X(k) + B*U(k)

[0102] where is a 3*3 identity matrix; T = 1 / 30 seconds represents the control period; U(k) = [dθ1(k), dθ2(k), dθ3(k)] T represents the system input, i.e., the angular velocity of each joint of the exoskeleton to be solved. Alternatively, the angular velocity is obtained by differentiating the joint position measured by the encoder, and the angular velocity is calculated by the exoskeleton controller, such as based on speed servo control, etc.

[0103] The model predictive control is equivalent to solving the following quadratic optimization equation:

[0104]

[0105] where p is a human given prediction step; X(k+i) ref = [q1(k)+i*dq1(k), q2(k)+i*dq2(k), q3(k)+i*dq3(k)] T , i=1...p is a reference trajectory composed of the healthy side upper limb shoulder joint abduction / adduction angle q1(k), shoulder joint flexion / extension angle q2(k), elbow joint flexion / extension angle q3(k) and the healthy side upper limb shoulder joint abduction / adduction angle velocity dq1(k), shoulder joint flexion / extension angle velocity dq2(k), elbow joint flexion / extension angle velocity dq3(k); Q i ∈R p×p and R i ∈R p×p is a human given diagonal weight matrix; U(k+i) = [dθ1(k+i), dθ2(k+i), dθ3(k+i)] T The constraint condition needs to be satisfied where dθ1 min , dθ1 max respectively represent the minimum and maximum angle velocities allowed for the exoskeleton shoulder joint abduction / adduction, dθ2 min , dθ2 max respectively represent the minimum and maximum angle velocities allowed for the exoskeleton shoulder joint flexion / extension, dθ3 min , dθ3 max respectively represent the minimum and maximum angle velocities allowed for the exoskeleton elbow joint flexion / extension, which depend on the hardware devices such as motors of the exoskeleton.

[0106] Set the prediction step p, the weight matrix Q i , R i and the constraint condition, substitute the quadratic optimization equation into the existing optimization algorithm library (such as the fmincon function in Matlab) for solving, and take the solved U(k+1) as the angle velocities of each joint of the exoskeleton, i.e. the exoskeleton shoulder joint abduction / adduction angle velocity is dθ1(k+1), the shoulder joint flexion / extension angle velocity is dθ2(k+1) and the elbow joint flexion / extension angle velocity is dθ3(k+1). The exoskeleton runs in a periodic synchronous speed mode and drives the hemiplegic side arm of the patient to the position mirroring the healthy side arm under the control of the exoskeleton controller according to the angle velocities.

[0107] Figure 5 The model predictive control effect comparison chart of the embodiment of the present application and the prior art is shown. The proportional control method in the prior art is compared with the predictive control effect of the embodiment, and the result is shown in Figure 5 .

[0108] Figure 5The curves in the figure represent the effects of different control modes on the exoskeleton arm in tracking the healthy side arm shoulder joint abduction / adduction angle q1, shoulder joint flexion / extension angle q2 and elbow joint flexion / extension angle q3 in 0-50s time. The black curve in the figure is the joint angle of the healthy side arm, i.e. the target angle that the exoskeleton needs to track; the star line in the figure is the exoskeleton arm joint angle obtained by using proportional control; and the dotted line in the figure is the exoskeleton arm joint angle obtained by using the proposed predictive control. From the figure, it can be seen that the angle error and delay of the predictive control are obviously smaller than those of the proportional control, and the specific data are shown in Table 2. Figure 5 It can be seen that the angle error and delay of the predictive control are obviously smaller than those of the proportional control, and the specific data are shown in Table 2.

[0109]

[0110] Table 2

[0111] The embodiment of the present application provides target-oriented upper limb mirror rehabilitation training, trains the cognitive ability of the patient while training the limb, and enables the patient to constantly think about how the healthy side arm moves during the training process, so that the undamaged half brain and the damaged brain interact, the intercortical inhibition is reduced, the activation amount of the cortical functional area of the patient is improved, and the activation area is expanded, so that the brain plasticity change and functional reorganization are more effectively promoted, and the function recovery of the damaged area is promoted.

[0112] The embodiment of the present application calculates the angular velocity of each joint of the robot based on the model predictive control algorithm. Due to the existence of the predictive model, the algorithm has a certain predictability, and can significantly reduce the algorithm delay and the angle error caused by the delay.

[0113] The embodiment of the present application integrates the calculation and smoothing of the joint angle set of the healthy side upper limb into one step of the unscented Kalman filter. Other methods first calculate the joint angle of the healthy side upper limb based on the geometric relationship, and then use the filtering algorithm for smoothing, which needs to be performed in two steps. The algorithm integration degree of the present application is higher, and the delay is smaller.

[0114] The embodiment of the present application only needs to capture the three-dimensional coordinates of the centers of three joints of the human upper limb, and other methods need to capture more joint centers, and the calculation amount is smaller.

[0115] The embodiment of the present application provides a rehabilitation robot, which comprises a visual sensor, an industrial control host, an exoskeleton and an exoskeleton controller. The visual sensor is used to acquire the three-dimensional coordinates of the centers of each joint of the patient's limb. The exoskeleton is used to apply an auxiliary torque to the limb to be rehabilitated of the patient to drive the limb to be rehabilitated to move. The industrial control host is used to execute the control method of the three-dimensional limb mirror rehabilitation robot. The exoskeleton controller is used to control the movement of the exoskeleton according to the angular velocity of each joint of the exoskeleton, so that the exoskeleton drives the limb to move to the position mirror of the healthy side limb.

[0116] Optionally, it further comprises a scene display module. The scene display module is used to display the mirror rehabilitation training scene.

[0117] Optionally, the exoskeleton controller comprises a periodic synchronization velocity mode for adjusting the angular velocity of each joint of the exoskeleton in real time.

[0118] The embodiment of the application further provides a computer readable storage medium, and a computer program is stored on the computer readable storage medium. The computer program is executed by a processor to implement each process of the control method of the three-dimensional limb mirror rehabilitation robot and achieve the same technical effects. To avoid repetition, details are not described herein. The computer readable storage medium is, for example, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, an optical disk, or the like.

[0119] Of course, those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by a computer degree to instruct a control device. The program can be stored in a computer readable storage medium. The program can include the processes of the above-mentioned method embodiments when executed. The storage medium can be a memory, a magnetic disk, an optical disk, or the like.

[0120] In this document, relational terms such as first and second and the like can be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises", "comprising", or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but can include other elements not expressly listed or inherent to such process, method, article, or apparatus. An element proceeded by "comprises a... " does not, without more constraints, exclude the existence of additional identical elements in the process, method, article, or apparatus that comprises the element.

[0121] The various embodiments in the specification are described in a progressive manner, and each embodiment focuses on the difference from other embodiments. The same or similar parts between the various embodiments can be referred to each other.

[0122] The above description of the disclosed embodiments enables a person skilled in the art to implement or use the application. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the application. Therefore, the application will not be limited to the embodiments shown herein, but will conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A rehabilitation robot characterized by, Comprise: a visual sensor, an industrial control host, an exoskeleton and an exoskeleton controller; the visual sensor is used to acquire three-dimensional coordinates of centers of each joint of a patient's limb; the exoskeleton is used to apply an auxiliary torque to the patient's limb to be rehabilitated to drive the limb to be rehabilitated to move; the industrial control host is used to execute a control method of a three-dimensional limb mirror rehabilitation robot; the exoskeleton controller is used to control the exoskeleton to move according to angular velocities of each joint of the exoskeleton, so that the exoskeleton drives the limb to move to a position mirroring a healthy side limb; wherein the control method of the three-dimensional limb mirror rehabilitation robot comprises: acquiring three-dimensional coordinates of centers of a plurality of joints of a healthy side limb; calculating and smoothing joint angles of the healthy side limb based on the three-dimensional coordinates and an unscented Kalman filtering algorithm to obtain angles and angular velocities of the plurality of joints; calculating angular velocities of corresponding joints of the exoskeleton based on the angles and angular velocities of the plurality of joints and a model predictive control algorithm; controlling the exoskeleton to move according to the angular velocities of the corresponding joints of the exoskeleton, so that the exoskeleton drives the limb to move to a position mirroring the healthy side limb; wherein the center of the joint comprises at least one of the following: a healthy side upper limb shoulder joint center, a healthy side upper limb elbow joint center and a healthy side upper limb wrist joint center; the calculating and smoothing of the joint angles of the healthy side limb based on the three-dimensional coordinates and the unscented Kalman filtering algorithm to obtain the angles and angular velocities of the plurality of joints comprises: inputting three-dimensional coordinates of the healthy side upper limb shoulder joint center, the healthy side upper limb elbow joint center and the healthy side upper limb wrist joint center at a current time into the unscented Kalman filtering algorithm to calculate and smooth joint angles of the healthy side upper limb, and outputting healthy side upper limb shoulder joint abduction / adduction angles, shoulder joint flexion / extension angles, elbow joint flexion / extension angles and healthy side upper limb shoulder joint abduction / adduction angular velocities, shoulder joint flexion / extension angular velocities and elbow joint flexion / extension angular velocities; wherein state variables of the healthy side upper limb system are as follows: wherein q1(k), q2(k) and q3(k) respectively represent healthy side upper limb shoulder joint abduction / adduction angles, shoulder joint flexion / extension angles and elbow joint flexion / extension angles at a current time k; dq1(k), dq2(k) and dq3(k) respectively represent healthy side upper limb shoulder joint abduction / adduction angular velocities, shoulder joint flexion / extension angular velocities and elbow joint flexion / extension angular velocities; x1(k), y1(k) and z1(k) respectively represent three-dimensional coordinates of a healthy side upper limb shoulder joint center S; dx1(k), dy1(k) and dz1(k) respectively represent three-dimensional translation velocities of the healthy side upper limb shoulder joint center S; L1(k) and L2(k) respectively represent lengths of a large arm and a small arm of the healthy side upper limb at the time k; assuming that each joint of the healthy side upper limb moves at a uniform speed and is subjected to random disturbance, a discrete state equation of the healthy side upper limb system is as follows: Wherein, k-1 represents the last time; T represents the control period; W(k) = [w1(k), w2(k), w3(k), w4(k), w5(k), w6(k)] T represents the random disturbance suffered by the system, all subject to normal distribution with zero mean; a system observation equation is obtained based on forward kinematics where Z(k) = [Z1(k), Z2(k), Z3(k), Z4(k), Z5(k), Z6(k), Z7(k), Z8(k), Z9(k)] T denotes the system observation variable; The actual observation quantity of the system is composed of three-dimensional coordinates [x1(k), y1(k), z1(k)] of the center of the shoulder joint of the healthy upper limb, three-dimensional coordinates [x2(k), y2(k), z2(k)] of the center of the elbow joint of the healthy upper limb, and three-dimensional coordinates [x3(k), y3(k), z3(k)] of the center of the wrist joint of the healthy upper limb; V(k) = [v1(k), v2(k), v3(k), v4(k), v5(k), v6(k), v7(k), v8(k), v9(k)] T denote the system measurement errors, all normally distributed with zero mean. Setting the initial value X(0) of the state variable, the variance of the random disturbance W(k) and the variance of the measurement error V(k), the discrete state equation, the system observation equation and the actual observation [x1(k), y1(k), z1(k), x2(k), y2(k), z2(k), x3(k), y3(k), z3(k)] T Substituting into the preset unscented Kalman algorithm for solving, the filtered healthy upper limb shoulder abduction / adduction angle q1(k), shoulder flexion / extension angle q2(k), elbow flexion / extension angle q3(k) and healthy upper limb shoulder abduction / adduction angle velocity dq1(k), shoulder flexion / extension angle velocity dq2(k), elbow flexion / extension angle velocity dq3(k) are solved out.

2. The rehabilitation robot according to claim 1, characterized in that, Based on the angles, angular velocities of the plurality of joints, and a model predictive control algorithm, the angular velocities of the corresponding joints of the exoskeleton are calculated, including: The abduction / adduction angle of the shoulder joint of the healthy upper limb, the forward / backward angle of the shoulder joint, the flexion / extension angle of the elbow joint, and the abduction / adduction angular velocity of the shoulder joint of the healthy upper limb, the forward / backward angular velocity of the shoulder joint, and the flexion / extension angular velocity of the elbow joint at the current time are input into a model predictive control algorithm, and the abduction / adduction angular velocity of the shoulder joint of the exoskeleton, the forward / backward angular velocity of the shoulder joint, and the flexion / extension angular velocity of the elbow joint are output.

3. The rehabilitation robot according to claim 2, characterized in that, State variable X(k) of the exoskeleton system = [θ1(k), θ2(k), θ3(k)] T ; wherein θ1(k), θ2(k), θ3(k) represent the abduction / adduction angle of the shoulder joint, the forward / backward angle of the shoulder joint, and the flexion / extension angle of the elbow joint at time k, respectively. The exoskeleton system prediction model is as follows: X(k+1) = A * X(k) + B * U(k) wherein is a 3*3 identity matrix; T denotes a control period; U(k) = [dθ1(k), dθ2(k), dθ3(k)] T denotes the joint angular velocities of the exoskeleton for the system input; The model predictive control is equivalent to solving the following quadratic optimization equation: wherein p is a preset prediction step; X(k+i) ref = [q1(k)+i*dq1(k),q2(k)+i*dq2(k),q3(k)+i*dq3(k)] T , i = 1...p is a reference trajectory composed of the shoulder abduction / adduction angle q1(k), the shoulder flexion / extension angle q2(k), the elbow flexion / extension angle q3(k) and the shoulder abduction / adduction velocity dq1(k), the shoulder flexion / extension velocity dq2(k), the elbow flexion / extension velocity dq3(k) of the healthy upper limb; Q i ∈ R p×p and R i ∈ R p×p is a preset diagonal weight matrix; U(k+i) = [dθ1(k+i), dθ2(k+i), dθ3(k+i)] T The constraint condition needs to be met wherein dθ1 min , dθ1 max respectively represent the minimum and maximum angular velocity allowed for the exoskeleton shoulder abduction / adduction, dθ2 min , dθ2 max respectively represent the minimum and maximum angular velocity allowed for the exoskeleton shoulder flexion / extension, dθ3 min , dθ3 max respectively represent the minimum and maximum angular velocity allowed for the exoskeleton elbow flexion / extension. Set the prediction step p, the weight matrix Q i ,R i and the constraint condition, substitute the quadratic optimization equation into the existing model predictive control algorithm to solve the angular velocity U(k+1) of each joint of the exoskeleton at the next moment, including the abduction / adduction angular velocity dθ1(k+1) of the shoulder joint of the exoskeleton, the forward / backward angular velocity dθ2(k+1) of the shoulder joint, and the flexion / extension angular velocity dθ3(k+1) of the elbow joint.

4. The rehabilitation robot according to claim 1, characterized in that, The exoskeleton is controlled according to the angular velocities of the corresponding joints of the exoskeleton to move the limb to a position mirroring the healthy limb, including: Based on the periodic synchronous speed mode and the angular velocities of the corresponding joints of the exoskeleton, the exoskeleton is controlled to move the limb to a position mirroring the healthy limb.

5. The rehabilitation robot according to claim 1, wherein, Further comprising a scene display module; The scene display module is used to display a mirroring rehabilitation training scene.

6. The rehabilitation robot according to claim 1, wherein, The exoskeleton controller includes a periodic synchronous speed mode for real-time adjustment of the angular velocities of the joints of the exoskeleton.

Images