Upper limb rehabilitation robot displacement compensation method, system, storage medium and computer

Abstract

The present invention provides a displacement compensation method, system, storage medium and computer for an upper limb rehabilitation robot. The compensation method comprises: establishing a rectangular coordinate system with a camera origin as the coordinate origin, and calculating the position deviation of an active capture point and a fixed capture point in the rectangular coordinate system; obtaining the torque of a rotating rod of the upper limb rehabilitation robot and two connecting rods connected to the rotating rod; calculating the visual control displacement deviation of a position image according to the position deviation, and determining the torque impedance adjustment displacement deviation according to the position deviation and the torque value; determining the coupling displacement deviation between torque and vision according to the visual control displacement deviation and the torque impedance adjustment displacement deviation; and determining the target compensation displacement of the upper limb rehabilitation robot according to the visual control displacement deviation, the torque impedance adjustment displacement deviation and the coupling displacement deviation. The torque compensation method provided by the present invention can automatically perform displacement compensation of the robot according to the patient's condition, and has a wide range of adaptability.

Description

Technical Field

[0001] The present invention relates to the technical field of medical devices, and in particular to a displacement compensation method, system, storage medium and computer for an upper limb rehabilitation robot. Background Art

[0002] Upper limb rehabilitation training mainly focuses on training the shoulder, elbow, wrist, and metacarpophalangeal and finger functions. Depending on the severity of the patient's illness, upper limb rehabilitation training includes active training of the patient and passive assisted training under the guidance of a rehabilitation trainer.

[0003] Normally, mild patients receive active training, and corresponding rehabilitation training can be carried out according to the patient's subjective consciousness. Severe patients receive passive assisted training. Currently, there are two main common upper limb rehabilitation treatment methods. One is to fix the patient's upper limbs to the end of the rehabilitation robot, and set the movement trajectory of the rehabilitation robot to train the patient's upper limbs. However, the movement trajectory of the rehabilitation robot is set in advance, and there is a lack of interaction with the patient, which makes it difficult to meet the patient's actual needs. The other is to have nurses take the patients for exercise training, and the patients interact with the nurses to help the patients complete the rehabilitation work. However, having nurses help with rehabilitation training requires a high level of experience for the nurses, and is difficult to promote. Summary of the invention

[0004] Based on this, the purpose of the present invention is to provide a displacement compensation method, system and storage medium for an upper limb rehabilitation robot to solve the technical problems existing in the prior art.

[0005] The present invention proposes a displacement compensation method for an upper limb rehabilitation robot, the upper limb rehabilitation robot comprises a four-link plane frame and a rotating rod, one end of the rotating rod away from the four-link plane frame is connected to a fixed rod through a first joint, one end of the rotating rod close to the four-link plane frame is rotatably connected to the end points of two connecting rods intersecting the four-link plane frame through a second joint, a camera and a fixed capture point of a signal are respectively arranged in front of the upper limb rehabilitation robot, the center point of the upper limb rehabilitation robot execution end is the active capture point of the signal, and the displacement compensation method for the upper limb rehabilitation robot comprises:

[0006] Establishing a rectangular coordinate system with the camera origin as the coordinate origin, acquiring position images of the active capture point and the fixed capture point through the camera, and calculating the position deviations of the active capture point and the fixed capture point in the rectangular coordinate system according to the position images;

[0007] Obtaining the torque of the upper limb rehabilitation robot's rotating rod and two connecting rods connected to the rotating rod and constructing a rotational joint torque function;

[0008] Calculate the visual control displacement deviation of the position image according to the position deviation, and determine the torque impedance adjustment displacement deviation according to the position deviation and the revolute joint torque function;

[0009] Determining a coupling displacement deviation between torque and vision according to the visual control displacement deviation and the torque impedance adjustment displacement deviation;

[0010] The target compensation displacement of the upper limb rehabilitation robot is determined according to the visual control displacement deviation, the torque impedance adjustment displacement deviation and the coupling displacement deviation.

[0011] Preferably, the step of calculating the position deviation of the active capture point and the fixed capture point in the rectangular coordinate system according to the position image comprises:

[0012] According to the position image, respectively obtain the corresponding coordinates and posture angles of the active capture point and the fixed capture point in the rectangular coordinate system;

[0013] Calculate the translation difference between the active capture point and the fixed capture point according to the coordinates of the active capture point and the fixed capture point in the rectangular coordinate system;

[0014] Determine a posture transformation matrix according to the posture angles of the active capture point and the fixed capture point in the rectangular coordinate system, and obtain a posture rotation matrix between the active capture point and the fixed capture point according to the posture transformation matrix;

[0015] The translation difference and the posture rotation matrix are used as position deviations of the active capture point and the fixed capture point in the rectangular coordinate system.

[0016] Preferably, the expression of the translation difference is:

[0017]

[0018] In the formula, are the coordinates of the active snap point in the rectangular coordinate system, are the coordinates of the fixed capture points in the rectangular coordinate system respectively;

[0019] The position and posture calculation expressions of the active capture point and the fixed capture point are as follows:

[0020]

[0021] In the formula, are the pose angles of the active capture point in the rectangular coordinate system, are the pose angles of the fixed capture point in the rectangular coordinate system, is the pose transformation matrix;

[0022]

[0023] The expression of the posture rotation matrix is:

[0024]

[0025] In the formula, are respectively the posture angles of the active capture point relative to the fixed capture point.

[0026] Preferably, the expression of the visual control displacement deviation is:

[0027]

[0028] In the formula, For visual control displacement deviation, is the scale factor of the position image control, is the motion vector image Jacobian matrix of the active capture point in the image space, is the translation difference.

[0029] Preferably, the step of determining the torque impedance to adjust the displacement deviation according to the position deviation and the revolute joint torque function comprises:

[0030] Obtaining the current rehabilitation needs of the patient, determining the expected torque values ​​of the rotating rod and the two connecting rods according to the rehabilitation needs, and constructing an expected rotational joint torque function;

[0031] A torque error control model is constructed according to the revolute joint torque function and the expected revolute joint torque function, and a torque impedance adjustment displacement deviation is determined according to the torque error control model.

[0032] Preferably, the expression of the torque impedance adjustment displacement deviation is:

[0033]

[0034] In the formula, Adjust displacement deviation for torque impedance, is the inertia matrix of the torque error control model; is the damping matrix of the torque error control model, is the environmental stiffness matrix, is the stiffness matrix of the torque error control model, is the revolute joint torque function, which is used to represent the current torque value of the rotating rod and the two connecting rods; is the expected revolute joint torque function, which is used to represent the expected torque value of the rotating rod and the two connecting rods;

[0035]

[0036] In the formula, is the torque of the rotating rod, , is the torque of the two connecting rods connected to the rotating rod;

[0037]

[0038] In the formula, is the desired torque of the rotating rod, , is the desired torque of the two connecting rods connected to the rotating rod.

[0039] Preferably, the expression of the coupling displacement deviation is:

[0040]

[0041] In the formula, is the coupling displacement deviation, is the coupling adjustment factor, is the pose rotation matrix between the active capture point and the fixed capture point, is the torque control direction matrix, is the position control direction matrix, For visual control displacement deviation, Adjust displacement deviation for torque impedance;

[0042] The target compensation displacement is expressed as:

[0043]

[0044] In the formula, To compensate for the displacement of the target, is the coupling displacement deviation, For visual control displacement deviation, Adjust displacement deviation for torque impedance.

[0045] The present invention also proposes a displacement compensation system for an upper limb rehabilitation robot, the upper limb rehabilitation robot comprising a four-link plane frame and a rotating rod, one end of the rotating rod away from the four-link plane frame is connected to a fixed rod through a first joint, one end of the rotating rod close to the four-link plane frame is rotatably connected to the end points of two connecting rods intersecting the four-link plane frame through a second joint, a camera and a fixed capture point of a signal are respectively arranged in front of the upper limb rehabilitation robot, the center point of the upper limb rehabilitation robot execution end is an active capture point of the signal, and the displacement compensation system for the upper limb rehabilitation robot comprises:

[0046] a calculation module, used to establish a rectangular coordinate system with the camera origin as the coordinate origin, obtain position images of the active capture point and the fixed capture point through the camera, and calculate the position deviations of the active capture point and the fixed capture point in the rectangular coordinate system according to the position images;

[0047] A construction module is used to obtain the torque of the upper limb rehabilitation robot's rotating rod and two connecting rods connected to the rotating rod and to construct a rotational joint torque function;

[0048] A first determination module is used to calculate the visual control displacement deviation of the position image according to the position deviation, and determine the torque impedance adjustment displacement deviation according to the position deviation and the revolute joint torque function;

[0049] A second determination module is used to determine the coupling displacement deviation between torque and vision according to the visual control displacement deviation and the torque impedance adjustment displacement deviation;

[0050] The third determination module is used to determine the target compensation displacement of the upper limb rehabilitation robot according to the visual control displacement deviation, the torque impedance adjustment displacement deviation and the coupling displacement deviation.

[0051] The present invention also provides a storage medium on which a computer program is stored. When the program is executed by a processor, the above-mentioned upper limb rehabilitation robot displacement compensation method is implemented.

[0052] The present invention also proposes a computer, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above-mentioned upper limb rehabilitation robot displacement compensation method when executing the computer program.

[0053] Compared with the prior art, the present invention has the following beneficial effects: the displacement compensation method for an upper limb rehabilitation robot proposed in the present application first establishes a rectangular coordinate system with the camera origin as the coordinate origin, obtains position images of active capture points and fixed capture points through the camera, and calculates the position deviations of the active capture points and fixed capture points in the rectangular coordinate system according to the position images; in a rehabilitation process, by obtaining the image between the robot execution end and the signal fixed capture point, the position of the fixed capture point relative to the camera is unchanged, and the position deviation between the two is calculated through the position image, which can adapt to patients with different body shapes and has a wide range of applications; further, the visual control displacement deviation of the position image is calculated according to the position deviation; the torque impedance adjustment displacement deviation is determined according to the position deviation and the torque value of the joint; and the two displacement deviations are coupled to calculate the coupling displacement deviation in the coupling process; based on the calculated visual control displacement deviation, torque impedance adjustment displacement deviation and coupling displacement deviation, the displacement compensation of the upper limb rehabilitation robot during the movement process is determined, thereby driving the patient to move according to the planned trajectory to achieve the best rehabilitation effect.

[0054] Additional aspects and advantages of the present invention will be given in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 This is a flow chart of the displacement compensation method for an upper limb rehabilitation robot in Embodiment 1 of the present invention;

[0056] Figure 2 Schematic diagram of the structure of the upper limb rehabilitation robot in Embodiment 1 of the present invention;

[0057] Figure 3 Schematic diagram of the relative positions of the upper limb rehabilitation robot and the camera in Embodiment 1 of the present invention;

[0058] Figure 4 This is a structural block diagram of a computer in Embodiment 4 of the present invention.

[0059] The following specific implementation manner will further illustrate the present invention in conjunction with the above-mentioned drawings. DETAILED DESCRIPTION

[0060] In order to facilitate the understanding of the present invention, the present invention will be described more fully below with reference to the relevant drawings. Several embodiments of the present invention are given in the drawings. However, the present invention can be implemented in many different forms and is not limited to the embodiments described herein. On the contrary, the purpose of providing these embodiments is to make the disclosure of the present invention more thorough and comprehensive.

[0061] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which the present invention belongs. The terms used herein in the specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The term "and / or" used herein includes any and all combinations of one or more of the related listed items.

[0062] Embodiment 1

[0063] See also Figure 1 , shown is the displacement compensation method of the upper limb rehabilitation robot in the first embodiment of the present invention. The upper limb rehabilitation robot terminal device has three degrees of freedom. Three servo motors drive the mechanical system to complete the upper limb rehabilitation training movement. The terminal actuator supports the patient's upper limb and moves along the desired trajectory to achieve the purpose of exercising the patient's muscles. This compensation method is mainly used for hemiplegic patients with certain motor abilities. It helps them complete upper limb rehabilitation by predicting the patient's active movement intention and movement trend. Figure 2 The figure shows the structural schematic diagram of the upper limb rehabilitation robot provided by the present embodiment, the upper limb rehabilitation robot comprises a fixed rod 1, a rotating rod 2 and a four-link planar frame composed of a connecting rod 3-connecting rod 6, the patient's upper limb is fixed to the end of the connecting rod 6; the fixed rod 1 is fixedly connected to the base, the fixed rod 1 is rotationally connected to the rotating rod 2 through a joint a, and the rotating rod 2 rotates around the z-axis; the connecting rod 3 and the connecting rod 5 are rotationally connected to the rotating rod 2 at joints b and b', and joints b and b' are overlapping joints in the vertical plane, the connecting rod 3 is located between joints b and joints f, and the connecting rod 5 is located between joints b' and joints d. The terminal device of the upper limb rehabilitation robot has three degrees of freedom, and the mechanical system is driven by three servo motors to complete the upper limb rehabilitation training movement. In the present embodiment, the three servo motors act on the rotating rod 2, the connecting rod 3 and the connecting rod 5 respectively, and the movement of the end of the connecting rod 6 is realized through the cooperation of the four-link planar frame and the rotating rod. The upper limb rehabilitation robot also includes a reducer and a torque sensor adapted to the servo motor. The torque sensor is used to measure the torque of the servo motor acting on the rotating rod 2, the connecting rod 3 and the connecting rod 5 respectively.

[0064] Specifically, the upper limb rehabilitation robot displacement compensation method specifically includes steps S10 to S50:

[0065] S10, establishing a rectangular coordinate system with the camera origin as the coordinate origin, acquiring position images of the active capture point and the fixed capture point through a camera, and calculating position deviations of the active capture point and the fixed capture point in the rectangular coordinate system according to the position images;

[0066] In specific implementation, Figure 3As shown, according to the image signal acquisition characteristics of the upper limb rehabilitation robot, a camera is set near the rehabilitation robot, and a rectangular coordinate system Oxyz is established with the origin O of the camera as the coordinate origin. A fixed capture point D of the signal is set at a fixed position at the front end of the upper limb rehabilitation robot. The capture point D can be set on the fixed seat of the robot. The center point of the upper limb rehabilitation robot execution end is set as the active capture point of the signal, and the corresponding rectangular coordinate systems Dxyz and Exyz are established. During each rehabilitation training process, due to the patient's physical differences such as height and arm length, as well as the relative position of the seat and the upper limb rehabilitation robot, the placement of the image recognition camera is different. Therefore, in order to more accurately calculate the coordinate position and posture equation of the center point E of the end actuator relative to point D, the calculation can be performed by referring to the camera signal point O.

[0067] Optionally, the step of calculating the position deviation of the active capture point and the fixed capture point in the rectangular coordinate system according to the position image includes:

[0068] According to the position image, respectively obtain the corresponding coordinates and posture angles of the active capture point and the fixed capture point in the rectangular coordinate system;

[0069] Calculate the translation difference between the active capture point and the fixed capture point according to the coordinates of the active capture point and the fixed capture point in the rectangular coordinate system;

[0070] Determine a posture transformation matrix according to the posture angles of the active capture point and the fixed capture point in the rectangular coordinate system, and obtain a posture rotation matrix between the active capture point and the fixed capture point according to the posture transformation matrix;

[0071] The translation difference and the posture rotation matrix are used as position deviations of the active capture point and the fixed capture point in the rectangular coordinate system.

[0072] In the specific implementation, the coordinates of point D in the Oxyz coordinate system are determined as , the corresponding posture angle is , the coordinates of point E in the Oxyz coordinate system are , the corresponding posture angle is ;

[0073] The expression for calculating the translation difference of point E relative to point D is:

[0074]

[0075] In the formula, are the coordinates of the active snap point in the rectangular coordinate system, are the coordinates of the fixed capture point in the rectangular coordinate system respectively; the translation difference can represent the actual running trajectory of the end effector;

[0076] The position and posture calculation expressions of the active capture point and the fixed capture point are as follows:

[0077]

[0078] In the formula, are the pose angles of the active capture point in the rectangular coordinate system, are the pose angles of the fixed capture point in the rectangular coordinate system, is the pose transformation matrix;

[0079]

[0080] The expression of the posture rotation matrix is:

[0081]

[0082] In the formula, are respectively the posture angles of the active capture point relative to the fixed capture point.

[0083] Finally, the position deviation expression of point E relative to point D is:

[0084]

[0085] In the formula, the parameters are similar to the above expressions and will not be repeated here; usually during a single rehabilitation training of the same patient, the camera position O and the capture point D remain fixed. The coordinate value and posture angle value of the end effector E of the upper limb rehabilitation robot relative to the fixed seat D of the robot can be solved according to the above calculation formula.

[0086] S20, obtaining the torque of the rotating rod of the upper limb rehabilitation robot and two connecting rods connected to the rotating rod and constructing a rotational joint torque function;

[0087] S30, calculating a visual control displacement deviation of the position image according to the position deviation, and determining a torque impedance adjustment displacement deviation according to the position deviation and the revolute joint torque function;

[0088] In the specific implementation, in order to make the movement of the end effector of the upper limb rehabilitation robot closer to the expected target point, the end position will move from the current position to the expected trajectory, and its posture angle direction is required to move in the direction of the expected trajectory. Considering the influence of the camera in taking images for trajectory analysis, in this embodiment, when calculating the displacement compensation of the upper limb rehabilitation robot, the influence of the position image visual control displacement deviation is considered; optionally, the expression of the visual control displacement deviation is:

[0089]

[0090] In the formula, For visual control displacement deviation, is the scale factor of the position image control, is the motion vector image Jacobian matrix of the active capture point in the image space, is the translation difference.

[0091] Further, the step of determining the torque impedance to adjust the displacement deviation according to the position deviation and the rotational joint torque function includes:

[0092] Obtaining the current rehabilitation needs of the patient, determining the expected torque values ​​of the rotating rod and the two connecting rods according to the rehabilitation needs, and constructing an expected rotational joint torque function;

[0093] A torque error control model is constructed according to the revolute joint torque function and the expected revolute joint torque function, and a torque impedance adjustment displacement deviation is determined according to the torque error control model.

[0094] The expression of the torque impedance adjustment displacement deviation is:

[0095]

[0096] In the formula, Adjust displacement deviation for torque impedance, is the inertia matrix of the torque error control model; is the damping matrix of the torque error control model, is the environmental stiffness matrix, is the stiffness matrix of the torque error control model, is the revolute joint torque function, which is used to represent the current torque value of the rotating rod and the two connecting rods; is the expected revolute joint torque function, which is used to represent the expected torque value of the rotating rod and the two connecting rods;

[0097]

[0098] In the formula, is the torque of the rotating rod, , is the torque of the two connecting rods connected to the rotating rod;

[0099]

[0100] In the formula, is the desired torque of the rotating rod, , is the desired torque of the two connecting rods connected to the rotating rod.

[0101] In the specific implementation, the torque sensor can be used to measure the torque of the upper limb rehabilitation robot's rotating rod and the two connecting rods connected to the rotating rod at the current moment. The torque values ​​are recorded as At this moment, the torque function relationship of each rotational joint of the upper limb rehabilitation robot is:

[0102]

[0103] According to the actual situation of the patient, the patient's rehabilitation needs are obtained, and then the expected torque corresponding to the rotating rod and the two connecting rods connected to the rotating rod is obtained. ;

[0104] In the motion control process of the upper limb rehabilitation robot, the upper limb rehabilitation robot end is required to be at the current position Move to the end position of the upper limb rehabilitation robot according to the planned motion trajectory , the expected trajectory of the upper limb rehabilitation robot execution end under the expected torque is During the movement, it is necessary to determine the torque impedance adjustment displacement deviation under the desired torque And compensate to meet the torque When the execution end device arrives , that is:

[0105]

[0106] Optionally, the expression of the torque error control model is:

[0107]

[0108] In the formula, Torque control error of upper limb rehabilitation robot, is the environmental stiffness matrix, and the torque impedance adjustment displacement deviation is obtained according to the torque error control model The expression between and torque is:

[0109]

[0110] In the formula, Adjust displacement deviation for torque impedance, is the inertia matrix of the torque error control model; is the damping matrix of the torque error control model, is the environmental stiffness matrix, is the stiffness matrix of the torque error control model, is the revolute joint torque function, which is used to represent the current torque value of the rotating rod and the two connecting rods; is the expected rotational joint torque function, which is used to represent the expected torque value of the rotating rod and the two connecting rods.

[0111] S40, determining a coupling displacement deviation between torque and vision according to the visual control displacement deviation and the torque impedance adjustment displacement deviation;

[0112] In order to improve the accuracy of the upper limb rehabilitation robot motion control, in this embodiment, torque and vision are combined to perform displacement compensation. Since there is a coupling effect between the two combined control methods, further compensation control is required; when the upper limb rehabilitation robot system performs constrained motion under the hybrid control algorithm, the coupling interference caused by the torque control variable and the position vision control variable needs to be calculated; further, in order to adapt to the influence of environmental parameter changes, the coupling factor Adjustment is made, therefore, in this embodiment, the expression of the coupling displacement deviation is obtained as follows:

[0113]

[0114] In the formula, is the coupling displacement deviation, is the coupling adjustment factor, is the pose rotation matrix between the active capture point and the fixed capture point, is the torque control direction matrix, is the position control direction matrix, For visual control displacement deviation, Adjust displacement deviation for torque impedance;

[0115] The target compensation displacement is expressed as:

[0116]

[0117] In the formula, To compensate for the displacement of the target, is the coupling displacement deviation, For visual control displacement deviation, Adjust displacement deviation for torque impedance.

[0118] In summary, the displacement compensation method for an upper limb rehabilitation robot proposed in the present application first establishes a rectangular coordinate system with the camera origin as the coordinate origin, obtains position images of active capture points and fixed capture points through the camera, and calculates the position deviations of the active capture points and fixed capture points in the rectangular coordinate system according to the position images; in a rehabilitation process, by obtaining the image between the robot execution end and the signal fixed capture point, the position of the fixed capture point relative to the camera is unchanged, and the position deviation between the two is calculated through the position image, which can adapt to patients with different body shapes and has a wide range of applications; further, the visual control displacement deviation of the position image is calculated according to the position deviation; the torque impedance adjustment displacement deviation is determined according to the position deviation and the torque value of the joint; and the two displacement deviations are coupled to calculate the coupling displacement deviation in the coupling process between the two; based on the calculated visual control displacement deviation, torque impedance adjustment displacement deviation and coupling displacement deviation, the displacement compensation of the upper limb rehabilitation robot during the movement process is determined, thereby driving the patient to move according to the planned trajectory to achieve the best rehabilitation effect.

[0119] In some optional embodiments, the sum of the upper limb force of the hemiplegic patient and the upper limb rehabilitation robot during rehabilitation training should be close to the force of normal upper limb movement. As the patient's upper limb muscle function gradually recovers and the movement stability becomes better and better, its displacement compensation value should gradually decrease; the torque impedance can be used to adjust the displacement deviation , expected trajectory and the current position of the end effector The running trajectory of is used as the evaluation index of upper limb movement stability after rehabilitation training; the expression of the evaluation index is:

[0120] .

[0121] Embodiment 2

[0122] The present embodiment provides an upper limb rehabilitation robot displacement compensation system, the upper limb rehabilitation robot comprises a four-link plane frame and a rotating rod, one end of the rotating rod away from the four-link plane frame is connected to a fixed rod through a first joint, one end of the rotating rod close to the four-link plane frame is rotatably connected to the end points of two connecting rods intersecting the four-link plane frame through a second joint, a camera and a fixed capture point of a signal are respectively arranged in front of the upper limb rehabilitation robot, the center point of the upper limb rehabilitation robot execution end is the active capture point of the signal, and the upper limb rehabilitation robot displacement compensation system comprises:

[0123] a calculation module, used to establish a rectangular coordinate system with the camera origin as the coordinate origin, obtain position images of the active capture point and the fixed capture point through the camera, and calculate the position deviations of the active capture point and the fixed capture point in the rectangular coordinate system according to the position images;

[0124] A construction module is used to obtain the torque of the upper limb rehabilitation robot's rotating rod and two connecting rods connected to the rotating rod and to construct a rotational joint torque function;

[0125] A first determination module is used to calculate the visual control displacement deviation of the position image according to the position deviation, and determine the torque impedance adjustment displacement deviation according to the position deviation and the revolute joint torque function;

[0126] A second determination module is used to determine the coupling displacement deviation between torque and vision according to the visual control displacement deviation and the torque impedance adjustment displacement deviation;

[0127] The third determination module is used to determine the target compensation displacement of the upper limb rehabilitation robot according to the visual control displacement deviation, the torque impedance adjustment displacement deviation and the coupling displacement deviation.

[0128] Preferably, the step of calculating the position deviation of the active capture point and the fixed capture point in the rectangular coordinate system according to the position image comprises:

[0129] According to the position image, respectively obtain the corresponding coordinates and posture angles of the active capture point and the fixed capture point in the rectangular coordinate system;

[0130] Calculate the translation difference between the active capture point and the fixed capture point according to the coordinates of the active capture point and the fixed capture point in the rectangular coordinate system;

[0131] Determine a posture transformation matrix according to the posture angles of the active capture point and the fixed capture point in the rectangular coordinate system, and obtain a posture rotation matrix between the active capture point and the fixed capture point according to the posture transformation matrix;

[0132] The translation difference and the posture rotation matrix are used as position deviations of the active capture point and the fixed capture point in the rectangular coordinate system.

[0133] Preferably, the expression of the translation difference is:

[0134]

[0135] In the formula, are the coordinates of the active snap point in the rectangular coordinate system, are the coordinates of the fixed capture points in the rectangular coordinate system respectively;

[0136] The position and posture calculation expressions of the active capture point and the fixed capture point are as follows:

[0137]

[0138] In the formula, are the pose angles of the active capture point in the rectangular coordinate system, are the pose angles of the fixed capture point in the rectangular coordinate system, is the pose transformation matrix;

[0139]

[0140] The expression of the posture rotation matrix is:

[0141]

[0142] In the formula, are respectively the posture angles of the active capture point relative to the fixed capture point.

[0143] Preferably, the expression of the visual control displacement deviation is:

[0144]

[0145] In the formula, For visual control displacement deviation, is the scale factor of the position image control, is the motion vector image Jacobian matrix of the active capture point in the image space, is the translation difference.

[0146] Preferably, the step of determining the torque impedance to adjust the displacement deviation according to the position deviation and the revolute joint torque function comprises:

[0147] Obtaining the current rehabilitation needs of the patient, determining the expected torque values ​​of the rotating rod and the two connecting rods according to the rehabilitation needs, and constructing an expected rotational joint torque function;

[0148] A torque error control model is constructed according to the revolute joint torque function and the expected revolute joint torque function, and a torque impedance adjustment displacement deviation is determined according to the torque error control model.

[0149] Preferably, the expression of the torque impedance adjustment displacement deviation is:

[0150]

[0151] In the formula, Adjust displacement deviation for torque impedance, is the inertia matrix of the torque error control model; is the damping matrix of the torque error control model, is the environmental stiffness matrix, is the stiffness matrix of the torque error control model, is the revolute joint torque function, which is used to represent the current torque value of the rotating rod and the two connecting rods; is the expected revolute joint torque function, which is used to represent the expected torque value of the rotating rod and the two connecting rods;

[0152]

[0153] In the formula, is the torque of the rotating rod, , is the torque of the two connecting rods connected to the rotating rod;

[0154]

[0155] In the formula, is the desired torque of the rotating rod, , is the desired torque of the two connecting rods connected to the rotating rod.

[0156] Preferably, the expression of the coupling displacement deviation is:

[0157]

[0158] In the formula, is the coupling displacement deviation, is the coupling adjustment factor, is the pose rotation matrix between the active capture point and the fixed capture point, is the torque control direction matrix, is the position control direction matrix, For visual control displacement deviation, Adjust displacement deviation for torque impedance;

[0159] The target compensation displacement is expressed as:

[0160]

[0161] In the formula, To compensate for the displacement of the target, is the coupling displacement deviation, For visual control displacement deviation, Adjust displacement deviation for torque impedance.

[0162] Embodiment 3

[0163] A third embodiment of the present invention provides a storage medium on which a computer program is stored. When the program is executed by a processor, the displacement compensation method of an upper limb rehabilitation robot as described above is implemented.

[0164] Embodiment 4

[0165] The present invention also provides a computer, see Figure 4 , shown is a computer in Embodiment 4 of the present invention, including a memory 10, a processor 20, and a computer program 30 stored in the memory 10 and executable on the processor 20. When the processor 20 executes the computer program 30, the above-mentioned upper limb rehabilitation robot displacement compensation method is implemented.

[0166] The memory 10 includes at least one type of storage medium, including flash memory, hard disk, multimedia card, card-type memory (e.g., SD or DX memory, etc.), magnetic memory, magnetic disk, optical disk, etc. In some embodiments, the memory 10 may be an internal storage unit of a computer, such as a hard disk of the computer. In other embodiments, the memory 10 may also be an external storage device, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card, etc. Further, the memory 10 may also include both an internal storage unit of the computer and an external storage device. The memory 10 may be used not only to store application software and various types of data installed in the computer, but also to temporarily store data that has been output or is to be output.

[0167] Among them, in some embodiments, the processor 20 can be an electronic control unit (Electronic Control Unit, abbreviated as ECU, also known as a vehicle computer), a central processing unit (Central Processing Unit, CPU), a controller, a microcontroller, a microprocessor or other data processing chip, used to run the program code stored in the memory 10 or process data, such as executing access restriction programs, etc.

[0168] It should be pointed out that Figure 4 The structure shown does not constitute a limitation on the computer. In other embodiments, the computer may include fewer or more components than shown in the figure, or combine certain components, or arrange the components differently.

[0169] Those skilled in the art will appreciate that the logic and / or steps represented in the flowchart or otherwise described herein, for example, may be considered as an ordered list of executable instructions for implementing logical functions, and may be specifically implemented in any computer-readable medium for use by an instruction execution system, device or apparatus (such as a computer-based system, a system including a processor, or other system that can fetch instructions from an instruction execution system, device or apparatus and execute instructions), or in conjunction with such instruction execution systems, devices or apparatuses. For purposes of this specification, "computer-readable medium" may be any device that can contain, store, communicate, propagate or transmit a program for use by an instruction execution system, device or apparatus, or in conjunction with such instruction execution systems, devices or apparatuses.

[0170] More specific examples of computer-readable media (a non-exhaustive list) include the following: an electrical connection with one or more wires (electronic device), a portable computer disk case (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disk read-only memory (CDROM). In addition, the computer-readable medium may even be a paper or other suitable medium on which the program is printed, since the program may be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, deciphering or, if necessary, processing in another suitable manner, and then stored in a computer memory.

[0171] It should be understood that the various parts of the present invention can be implemented by hardware, software, firmware or a combination thereof. In the above-mentioned embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or a combination thereof: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0172] The technical features of the above-described embodiments may be arbitrarily combined. To make the description concise, not all possible combinations of the technical features in the above-described embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0173] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

Claims

1. A displacement compensation method for an upper limb rehabilitation robot, characterized in that: The upper limb rehabilitation robot comprises a four-link plane frame and a rotating rod, one end of the rotating rod away from the four-link plane frame is connected to a fixed rod through a first joint, and one end of the rotating rod close to the four-link plane frame is rotatably connected to the end points of two connecting rods intersecting the four-link plane frame through a second joint, a camera and a fixed capture point of a signal are respectively arranged in front of the upper limb rehabilitation robot, and the center point of the upper limb rehabilitation robot execution end is the active capture point of the signal. The displacement compensation method of the upper limb rehabilitation robot comprises: Establishing a rectangular coordinate system with the camera origin as the coordinate origin, acquiring position images of the active capture point and the fixed capture point through the camera, and calculating the position deviations of the active capture point and the fixed capture point in the rectangular coordinate system according to the position images; Obtaining the torque of the upper limb rehabilitation robot's rotating rod and two connecting rods connected to the rotating rod and constructing a rotational joint torque function; Calculate the visual control displacement deviation of the position image according to the position deviation, and determine the torque impedance adjustment displacement deviation according to the position deviation and the revolute joint torque function; Determining a coupling displacement deviation between torque and vision according to the visual control displacement deviation and the torque impedance adjustment displacement deviation; Determining a target compensation displacement of an upper limb rehabilitation robot according to the visual control displacement deviation, the torque impedance adjustment displacement deviation, and the coupling displacement deviation; The expression of the coupling displacement deviation is: In the formula, is the coupling displacement deviation, is the coupling adjustment factor, is the pose rotation matrix between the active capture point and the fixed capture point, is the torque control direction matrix, is the position control direction matrix, For visual control displacement deviation, Adjust displacement deviation for torque impedance.

2. The upper limb rehabilitation robot displacement compensation method according to claim 1, characterized in that: The step of calculating the position deviation of the active capture point and the fixed capture point in the rectangular coordinate system according to the position image comprises: According to the position image, respectively obtain the corresponding coordinates and posture angles of the active capture point and the fixed capture point in the rectangular coordinate system; Calculate the translation difference between the active capture point and the fixed capture point according to the coordinates of the active capture point and the fixed capture point in the rectangular coordinate system; Determine a posture transformation matrix according to the posture angles of the active capture point and the fixed capture point in the rectangular coordinate system, and obtain a posture rotation matrix between the active capture point and the fixed capture point according to the posture transformation matrix; The translation difference and the posture rotation matrix are used as position deviations of the active capture point and the fixed capture point in the rectangular coordinate system.

3. The displacement compensation method for an upper limb rehabilitation robot according to claim 2, characterized in that: The expression of the translation difference is: In the formula, are the coordinates of the active snap point in the rectangular coordinate system, are the coordinates of the fixed capture points in the rectangular coordinate system respectively; The position and posture calculation expressions of the active capture point and the fixed capture point are as follows: In the formula, are the pose angles of the active capture point in the rectangular coordinate system, are the pose angles of the fixed capture point in the rectangular coordinate system, is the pose transformation matrix; The expression of the posture rotation matrix is: In the formula, are respectively the posture angles of the active capture point relative to the fixed capture point.

4. The displacement compensation method for an upper limb rehabilitation robot according to claim 3, characterized in that: The expression of the visual control displacement deviation is: In the formula, For visual control displacement deviation, is the scale factor of the position image control, is the motion vector image Jacobian matrix of the active capture point in the image space, is the translation difference.

5. The displacement compensation method for an upper limb rehabilitation robot according to claim 4, characterized in that: The step of determining the torque impedance according to the position deviation and the revolute joint torque function to adjust the displacement deviation comprises: Acquire current rehabilitation needs, determine expected torque values ​​of the rotating rod and the two connecting rods according to the rehabilitation needs, and construct an expected rotational joint torque function; A torque error control model is constructed according to the revolute joint torque function and the expected revolute joint torque function, and a torque impedance adjustment displacement deviation is determined according to the torque error control model.

6. The upper limb rehabilitation robot displacement compensation method according to claim 5, characterized in that: The expression of the torque impedance adjustment displacement deviation is: In the formula, Adjust displacement deviation for torque impedance, is the inertia matrix of the torque error control model; is the damping matrix of the torque error control model, is the environmental stiffness matrix, is the stiffness matrix of the torque error control model, is the revolute joint torque function, which is used to represent the current torque value of the rotating rod and the two connecting rods; is the expected revolute joint torque function, which is used to represent the expected torque value of the rotating rod and the two connecting rods; In the formula, is the torque of the rotating rod, , is the torque of the two connecting rods connected to the rotating rod; In the formula, is the desired torque of the rotating rod, , is the desired torque of the two connecting rods connected to the rotating rod.

7. The upper limb rehabilitation robot displacement compensation method according to claim 6, characterized in that: The target compensation displacement is expressed as: In the formula, To compensate for the displacement of the target, is the coupling displacement deviation, For visual control displacement deviation, Adjust displacement deviation for torque impedance.

8. A displacement compensation system for an upper limb rehabilitation robot, characterized in that: The upper limb rehabilitation robot comprises a four-link plane frame and a rotating rod, one end of the rotating rod away from the four-link plane frame is connected to a fixed rod through a first joint, and one end of the rotating rod close to the four-link plane frame is rotatably connected to the end points of two connecting rods intersecting the four-link plane frame through a second joint, a camera and a fixed capture point of a signal are respectively arranged in front of the upper limb rehabilitation robot, the center point of the upper limb rehabilitation robot execution end is the active capture point of the signal, and the upper limb rehabilitation robot displacement compensation system comprises: a calculation module, used to establish a rectangular coordinate system with the camera origin as the coordinate origin, obtain position images of the active capture point and the fixed capture point through the camera, and calculate the position deviations of the active capture point and the fixed capture point in the rectangular coordinate system according to the position images; A construction module is used to obtain the torque of the upper limb rehabilitation robot's rotating rod and two connecting rods connected to the rotating rod and to construct a rotational joint torque function; A first determination module is used to calculate the visual control displacement deviation of the position image according to the position deviation, and determine the torque impedance adjustment displacement deviation according to the position deviation and the revolute joint torque function; A second determination module is used to determine the coupling displacement deviation between torque and vision according to the visual control displacement deviation and the torque impedance adjustment displacement deviation; A third determination module is used to determine the target compensation displacement of the upper limb rehabilitation robot according to the visual control displacement deviation, the torque impedance adjustment displacement deviation and the coupling displacement deviation; The expression of the coupling displacement deviation is: In the formula, is the coupling displacement deviation, is the coupling adjustment factor, is the pose rotation matrix between the active capture point and the fixed capture point, is the torque control direction matrix, is the position control direction matrix, For visual control displacement deviation, Adjust displacement deviation for torque impedance.

9. A storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the upper limb rehabilitation robot displacement compensation method as described in any one of claims 1 to 7 is implemented.

10. A computer comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the upper limb rehabilitation robot displacement compensation method as described in any one of claims 1 to 7 is implemented.

Images