Upper limb rehabilitation robot based on parallelogram joints and dynamic modeling method

Abstract

The invention discloses an upper limb rehabilitation robot based on parallelogram joints and a dynamic modeling method. The upper limb rehabilitation robot comprises a base, a back structure, shoulder structures, large arms and small arms. The back structure comprises a first back joint module, a back structural part, a second back joint module, a first back guide rail and a second back guide rail; the upper end of the back structural part is connected with the first back joint module, the second back joint module is connected with the lower end of the back structural part and fixedly connected with one end of the second back guide rail, and the second back guide rail is slidably connected with a plane bearing. One end of the first back guide rail is rotationally connected with the back structural part, plane bearings are slidably connected to the first back guide rail, and the first back guide rail, the second back guide rail and the two plane bearings jointly form a parallelogram joint; the axes of the three joint modules of the shoulder structure intersect at one point, and the point and the center connecting line of the two plane bearings of the shoulder structure are collinear. Dynamic changes of the center of the glenohumeral joint in the movement process of the upper limbs of the human body are fitted through movement of the parallelogram joints, and forward bending and backward stretching movement of the shoulders of the human body in the horizontal plane is achieved.

Description

Technical Field

[0001] The invention belongs to the technical field of rehabilitation robots, and in particular is an upper limb rehabilitation robot based on parallelogram joints and a dynamic modeling method. Background Art

[0002] The upper limb rehabilitation robot is based on the principles of bionics and is designed based on the physiological structure of human limbs to fit the mechanical structure of the human upper limbs. It supports and drives the joints of the wrist, arm and other parts of the upper limbs, and helps patients recover upper limb function by providing appropriate assistance and resistance.

[0003] The human shoulder is a complex structure with multiple degrees of freedom composed of multiple joints and muscles. Traditional upper limb rehabilitation robots generally use motors with three intersecting axes to fit the degrees of freedom of the glenohumeral joint of the human shoulder. In fact, the position of the center of the glenohumeral joint changes dynamically during the movement of the human upper limb. The motion trajectory of the existing upper limb rehabilitation robot has a low degree of fit with the motion trajectory of the human upper limb, resulting in poor training effects and easy secondary injuries to patients. Based on this, the present invention proposes an upper limb rehabilitation robot based on parallelogram joints and a dynamic modeling method, which improves the fitting degree of the upper limb rehabilitation robot and increases the flexibility of the motion trajectory. Summary of the invention

[0004] In view of the deficiencies in the prior art, the technical problem to be solved by the present invention is to provide an upper limb rehabilitation robot based on parallelogram joints and a dynamic modeling method.

[0005] The present invention solves the technical problem by adopting the following technical solution:

[0006] On the one hand, the present invention provides an upper limb rehabilitation robot based on a parallelogram joint, comprising a base, a back structure, a shoulder structure, an upper arm and a lower arm; the back structure comprises a first back joint module, a back structural member, a second back joint module, a first back guide rail and a second back guide rail; the shoulder structure comprises a first shoulder joint module, a first shoulder structural member, a second shoulder structural member, a second shoulder joint module, a third shoulder structural member and a third shoulder joint module;

[0007] The first back joint module is located on the base, the axis of the first back joint module coincides with the sagittal axis of the human body, the upper end of the back structure is connected to the load end of the first back joint module, the second back joint module is connected to the lower end of the back structure, the load end of the second back joint module is fixedly connected to one end of the second back guide rail, and a plane bearing is slidably connected to the second back guide rail; one end of the first back guide rail is rotatably connected to the back structure, and a plane bearing is slidably connected to the first back guide rail; the connection center of the first back guide rail and the back structure, the connection center of the second back guide rail and the second back joint module, and the geometric centers of the two plane bearings correspond to the four vertices of the parallelogram, so the first back guide rail, the second back guide rail and the two plane bearings together constitute a parallelogram joint;

[0008] The lower end of the first shoulder structural member is connected to the two plane bearings of the shoulder structure at the same time, the first shoulder joint module is connected to the upper end of the first shoulder structural member and the lower end of the second shoulder structural member at the same time, the second shoulder joint module is connected to the upper end of the second shoulder structural member and the upper end of the third shoulder structural member at the same time, the third shoulder joint module is connected to the lower end of the third shoulder structural member and the upper end of the upper arm at the same time, the axes of the first shoulder joint module, the second shoulder joint module and the third shoulder joint module intersect at one point, and the point is collinear with the center line of the two plane bearings of the shoulder structure.

[0009] Furthermore, the upper arm includes a first upper arm structural member, a second upper arm structural member, an upper arm joint module and an upper arm fixing member; the upper end of the first upper arm structural member is connected to the third shoulder joint module, the second upper arm structural member is inserted into the first upper arm structural member and the insertion depth is adjustable, and the upper arm fixing member is located on the inner side of the second upper arm structural member; the upper arm joint module is simultaneously connected to the lower end of the second upper arm structural member and the upper end of the forearm.

[0010] On the other hand, the present invention also provides a dynamic modeling method for an upper limb rehabilitation robot based on a parallelogram joint, comprising the following steps:

[0011] Step 1: Establish the base coordinate system and joint coordinate system;

[0012] A base coordinate system O0-X0Y0Z0 is established with the intersection of the sagittal axis and the coronal axis of the human body as the origin. The Z0 axis of the base coordinate system is collinear with the axis of the first joint module of the back and points to the first joint module of the back; the Z0 axis of the base coordinate system is rotated -90° around the coronal axis of the human body to obtain the first joint coordinate system O1-X1Y1Z1; the second joint coordinate systems O2-X2Y2Z2 to the fourth joint coordinate systems O4-X4Y4Z4 are established with the intersection of the axes of the first joint module of the shoulder, the second joint module of the shoulder and the third joint module of the shoulder as the origin. The Z2 axis of the second joint coordinate system is collinear with the axis of the first joint module of the shoulder and points to the first joint module of the shoulder, and the Z3 axis of the third joint coordinate system is collinear with the axis of the first joint module of the shoulder and points to the first joint module of the shoulder. The Z3 axis of the system is collinear with the axis of the second joint module of the shoulder and points to the second joint module of the shoulder, and the Z4 axis of the fourth joint coordinate system is collinear with the axis of the third joint module of the shoulder and points to the third joint module of the shoulder; the fifth joint coordinate system O5-X5Y5Z5 is established with the intersection of the long center line of the inner side of the upper arm and the axis of the upper arm joint module as the origin, and the Z5 axis of the fifth joint coordinate system is collinear with the long center line of the inner side of the upper arm and points to the forearm; the Y2 axis of the second joint coordinate system is used as the rotation axis, and the rotation angle is γ to obtain the intermediate coordinate system O2'-X2'Y2'Z2', and the Z2' axis of the intermediate coordinate system is collinear with the center line of the two plane bearings of the shoulder structure and points to the two plane bearings;

[0013] Step 2: Use the standard DH parameter method to perform forward kinematic modeling on the upper limb rehabilitation robot, and obtain the rotation and position transformation relationship from the fifth joint coordinate system to the base coordinate system:

[0014]

[0015]

[0016] In the formula, is the rotation matrix from the fifth joint coordinate system to the base coordinate system; Indicates that the i-th joint coordinate system is respectively Z i and X i The axis is the rotation axis and the rotation angle is θ i , α i The rotation matrix, i = 1, 2, 3, 4, 5; θ i is the angle of the i-th joint, α i Indicates around X i Axis from Z i-1 Axis rotation to Z i Angle of axis, Y γ represents the rotation matrix between the intermediate coordinate system and the second joint coordinate system, 0 P. 5 P is the position of any point P on the robot in the base coordinate system and the fifth joint coordinate system, lh is the length of the upper arm, l p It is the distance between the connection center of the back second guide rail and the back second joint module and the center of the plane bearing located on the second guide rail;

[0017] Step 3: Solve the inverse kinematics according to the following formula to obtain the joint angles θ1~θ5;

[0018]

[0019] In the formula, 0 O5 is the position matrix of the origin of the fifth joint coordinate system relative to the base coordinate system. 0 O 2,3,4 is the position matrix of the origin of the second joint coordinate system to the fourth joint coordinate system relative to the base coordinate system;

[0020] Step 4: Conduct inverse dynamics modeling of the upper limb rehabilitation robot to obtain the spatial velocity and spatial force of the terminal joint;

[0021] Space velocity of the end joint It is expressed as:

[0022]

[0023] In the formula, 0 ω is the angular velocity of the end joint, 0 v O is the linear velocity of the origin O of the fifth joint coordinate system;

[0024] Spatial force at the distal joint It is expressed as:

[0025]

[0026] In the formula, is a space vector describing external forces and torques, F and M are external forces and torques applied to the end joints, 0 ω F is the angular velocity at the point of application of the external force, 0 ω M is the angular velocity at the point of action of the torque, 0 ρ F is the position vector from the reference point to the point where the external force acts, is the zero vector.

[0027] Furthermore, in the first step, two adjacent frames of images are randomly intercepted during the movement of the upper limb rehabilitation robot, and according to the position of any point P on the robot in the two frames of images, the transformation relationship between the second joint coordinate system and the first joint coordinate system is obtained as follows:

[0028]

[0029] In the formula, 1 P. 2 P is the position of point P in the first and second joint coordinate systems, 2 ′P is the position of point P in the intermediate coordinate system; α2 is the angle between the first joint coordinate system and the second joint coordinate system Z axis along the X2 axis of the second joint coordinate system; θ2 is the angle between the first joint coordinate system and the second joint coordinate system X axis along the Z1 axis of the first joint coordinate system; c means taking the cosine, and s means taking the sine;

[0030] The transformation relationship between two adjacent joint coordinate systems is expressed as:

[0031]

[0032] In the formula, i-1 P. i P is the position of point P in the i-1th, i-th joint coordinate system, a i Indicates that along X i Axis from Z i-1 Axis moves to Z i Axis distance, d i Indicates that along Z i-1 Axis from X i-1 Axis moves to X i Axis distance.

[0033] Compared with the prior art, the present invention has the following beneficial effects:

[0034] 1. The parallelogram joint is regarded as a multi-link structure with only one joint variable. The back structure is designed with a parallelogram joint to fit the human glenohumeral joint. The movement of the parallelogram joint fits the dynamic changes of the center of the glenohumeral joint during the movement of the human upper limbs, and realizes the flexion and extension movement of the human shoulder in the horizontal plane. The joint module is flexible, which enables the upper limb rehabilitation robot to have the ability to buffer and absorb impact force, improve the flexibility of the robot's movement, and avoid secondary injuries to the user.

[0035] 2. In traditional robot kinematics, when the DH parameter method is used to model the robot, the joints are divided into two types: rotation and movement, and the joint coordinate systems are all established on the revolute pair or the translation pair. Although the parallelogram joint has 4 revolute pairs, it has only one degree of freedom. If the traditional DH parameter method is used to model the parallelogram joint, it is necessary to establish a coordinate system on each of the 4 revolute pairs, which not only leads to a complex model and a large amount of calculation, but also makes it difficult to accurately describe the motion law of the parallelogram joint. On the basis of fully analyzing the motion law of the parallelogram joint, the present invention abstracts the 4 coordinate systems located on the parallelogram joint by the traditional DH parameter method into two coordinate systems (the first joint coordinate system and the intermediate coordinate system) established outside the parallelogram joint, greatly reducing the amount of calculation, and can more accurately describe the motion posture of the robot based on the parallelogram joint, solving the problem that the traditional modeling method is difficult to simply and accurately describe the motion of the parallelogram joint. At the same time, by adopting the recursive Newton-Euler method combined with the spatial dynamics representation, the efficient and accurate dynamic modeling of the upper limb rehabilitation robot based on the parallelogram joint is realized, which is suitable for real-time control environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 This is a schematic diagram of the overall structure of the upper limb rehabilitation robot;

[0037] Figure 2 is a schematic diagram of the back structure;

[0038] Figure 3 is a schematic diagram of the shoulder structure;

[0039] Figure 4 It is a schematic diagram of the structure of the big arm at one viewing angle;

[0040] Figure 5 This is a schematic diagram of the structure of the upper arm from another perspective;

[0041] Figure 6 It is a schematic diagram of the structure of the forearm;

[0042] Figure 7 is a schematic diagram of the joint coordinate system;

[0043] In the figure, 1-base; 2-back structure; 3-shoulder structure; 4-upper arm; 5-lower arm;

[0044] 201- first joint module of back; 202- structural member of back; 203- second joint module of back; 204- second guide rail of back; 205- first plane bearing; 206- first guide rail of back; 207- first bearing fixing seat; 208- second bearing fixing seat; 209- first stopper; 210- second stopper; 211- second plane bearing; 212- third plane bearing; 301- first joint module of shoulder; 302- first structural member of shoulder; 303- second structural member of shoulder; 304- second joint module of shoulder Section module; 305-shoulder third structural part; 306-shoulder third joint module; 401-screw; 402-big arm first guide rail; 403-big arm second guide rail; 404-big arm first structural part; 405-worm gear; 406-worm; 407-first bearing seat; 408-second bearing seat; 409-handwheel; 410-big arm second structural part; 411-big arm joint module; 412-six-dimensional force sensor; 413-big arm fixing part; 414-slider; 501-forearm fixing part; 502-forearm structural part. DETAILED DESCRIPTION

[0045] Specific embodiments are given below in conjunction with the accompanying drawings. The specific embodiments are only used to further describe the technical solutions of the present invention in detail and are not intended to limit the protection scope of the present application.

[0046] The present invention provides an upper limb rehabilitation robot based on a parallelogram joint, comprising a base 1, a back structure 2, a shoulder structure 3, an upper arm 4 and a lower arm 5;

[0047] The back structure 2 includes a back first joint module 201, a back structure 202, a back second joint module 203, a first plane bearing 205, a second plane bearing 211, a third plane bearing 212, a back first guide rail 206 and a back second guide rail 204;

[0048] Among them, the first back joint module 201 is fixed on the base 1, the axis of the first back joint module 201 coincides with the sagittal axis of the human body, the load end of the first back joint module 201 is connected to the upper end of the back structure 202, and the back structure 202 is driven to rotate around the base 1 by the first back joint module 201, so that the upper limb rehabilitation robot can simulate the lifting and lowering movement of the human shoulder in the sagittal plane; the second back joint module 203 is fixed to the lower end of the back structure 202, and the load end of the second back joint module 203 is connected to the first back joint module 201. One end of the second guide rail 204 is fixedly connected, and a third plane bearing 212 is slidably connected to the second guide rail 204 on the back through a second bearing fixing seat 208, and the third plane bearing 212 can rotate relative to the second bearing fixing seat 208, and the other end of the second guide rail 204 on the back is provided with a second limiter 210 for limiting the second bearing fixing seat 208; one end of the first guide rail 206 on the back is rotatably connected to the back structure 202 through the first plane bearing 205, and the first guide rail 206 on the back is slidably connected to the third plane bearing 212 through the first bearing fixing seat 207. The first plane bearing 211 is provided with a second plane bearing 211, and the second plane bearing 211 can rotate relative to the first bearing fixing seat 207, and the other end of the first back guide rail 206 is provided with a first limiter 209 for limiting the first bearing fixing seat 207; the connection center of the first back guide rail 206 and the first plane bearing 205, the connection center of the second back guide rail 204 and the second back joint module 203, the geometric center of the second plane bearing 211 and the geometric center of the third plane bearing 212 are the four vertices of the parallelogram, so the first back guide rail 206 The second guide rail 204 on the back, the second plane bearing 211, the third plane bearing 212, the first bearing fixing seat 207 and the second bearing fixing seat 208 together constitute a parallelogram joint, which is used to fit the human glenohumeral joint. The rotation of the parallelogram joint can simultaneously realize movement along the x and y directions, thereby fitting the dynamic changes of the center position of the glenohumeral joint during the movement of the upper limbs of the human body; the parallelogram joint is driven to rotate by the second joint module 203 on the back, so that the upper limb rehabilitation robot can simulate the flexion and tension movement of the human shoulder in the horizontal plane.

[0049] The shoulder structure 3 includes a first shoulder joint module 301, a first shoulder structure 302, a second shoulder structure 303, a second shoulder joint module 304, a third shoulder structure 305 and a third shoulder joint module 306;

[0050] Among them, the lower end of the first shoulder structure 302 is connected to the second plane bearing 211 and the third plane bearing 212 at the same time, the first shoulder joint module 301 is connected to the upper end of the first shoulder structure 302 and the lower end of the second shoulder structure 303 at the same time, the second shoulder joint module 304 is connected to the upper end of the second shoulder structure 303 and the upper end of the third shoulder structure 305 at the same time, the third shoulder joint module 306 is connected to the lower end of the third shoulder structure 305 and the upper end of the first arm structure 404 at the same time, the axes of the first shoulder joint module 301, the second shoulder joint module 304 and the third shoulder joint module 306 intersect at one point, and the point is collinear with the center line of the second plane bearing 211 and the third plane bearing 212, so that the upper limb rehabilitation robot can simulate the movement of the human glenohumeral joint.

[0051] The arm 4 includes a lead screw 401, a first arm guide rail 402, a second arm guide rail 403, a first arm structure 404, a worm wheel 405, a worm 406, a first bearing seat 407, a second bearing seat 408, a hand wheel 409, a second arm structure 410, an arm joint module 411, a six-dimensional force sensor 412, an arm fixing member 413 and a slider 414;

[0052] The lead screw 401 is vertically mounted on the first arm structure 404, the first arm guide rail 402 and the second arm guide rail 403 are symmetrically distributed on both sides of the lead screw 401 and are fixedly connected to the first arm structure 404, and the slider 414 is slidably connected to the lead screw 401, the first arm guide rail 402 and the second arm guide rail 403 at the same time; the worm 406 is located at the upper part of the first arm structure 404, and the two ends are connected to the first arm structure 404 through the first bearing seat 407 and the second bearing seat 408 respectively, so that the worm 406 can move relative to the first arm structure 404. The first structure 404 of the boom rotates; the hand wheel 409 is located at one end of the worm 406 and is used to manually rotate the worm 406; the worm wheel 405 is located at the upper end of the lead screw 401 and meshes with the worm 406; the upper end of the second structure 410 of the boom extends into the first structure 404 of the boom and is connected to the slider 414, and the boom joint module 411 is simultaneously connected to the lower end of the second structure 410 of the boom and the upper end of the small arm structure 502; the boom fixing part 413 is installed in the middle of the first structure 404 of the boom through the six-dimensional force sensor 412. The lead screw 401 is driven to rotate by the worm 406 and the worm wheel 405, so that the slider 414 slides on the lead screw 401 to adjust the length of the second structure 410 of the boom extending into the first structure 404 of the boom, thereby adjusting the length of the boom.

[0053] The forearm 5 includes a forearm fixing part 501 and a forearm structural part 502 ; the forearm fixing part 501 is located in the middle of the forearm structural part 502 and is used for connecting the forearm of the user.

[0054] The present invention also provides a method for dynamic modeling of an upper limb rehabilitation robot based on a parallelogram joint, comprising the following steps:

[0055] Step 1: Establish the base coordinate system and joint coordinate system;

[0056] The base coordinate system O0-X0Y0Z0 is established with the intersection of the sagittal axis and the coronal axis of the human body as the origin, and the Z0 axis of the base coordinate system is collinear with the axis of the first back joint module 201 and points to the first back joint module 201. The Z0 axis of the base coordinate system is rotated -90° around the coronal axis of the human body to obtain the first joint coordinate system O1-X1Y1Z1. The second joint coordinate system O2-X2Y2Z2 to the fourth joint coordinate system O4-X4Y4Z4 are respectively established with the intersection of the axis of the first shoulder joint module 301, the second shoulder joint module 304 and the third shoulder joint module 306 as the origin, the Z2 axis of the second joint coordinate system is collinear with the axis of the first shoulder joint module 301 and points to the first shoulder joint module 301, the Z3 axis of the third joint coordinate system is collinear with the axis of the second shoulder joint module 304 and points to the second shoulder joint module 304, and the Z4 axis of the fourth joint coordinate system is collinear with the axis of the third shoulder joint module 306 and points to the third shoulder joint module 306. The fifth joint coordinate system O5-X5Y5Z5 is established with the intersection of the long center line of the inner side of the upper arm and the axis of the upper arm joint module 411 as the origin, and the Z5 axis of the fifth joint coordinate system is collinear with the long center line of the inner side of the upper arm and points to the forearm. The Y2 axis of the second joint coordinate system is used as the rotation axis, and the rotation angle is γ to obtain the intermediate coordinate system O2'-X2'Y2'Z2'. The Z2' axis of the intermediate coordinate system is colinear with the center line of the second plane bearing 211 and the third plane bearing 212 and points to the two plane bearings.

[0057] Step 2: Use the standard DH parameter method to perform forward kinematic modeling on the upper limb rehabilitation robot, and obtain the rotation and position transformation relationship from the fifth joint coordinate system to the base coordinate system;

[0058] During the movement of the upper limb rehabilitation robot, two adjacent frames of images are randomly intercepted. According to the position of any point P on the robot in the two frames of images, the transformation relationship between the second joint coordinate system and the first joint coordinate system is obtained as follows:

[0059]

[0060] In the formula, 1 P. 2 P is the position of point P in the first and second joint coordinate systems, 2′P is the position of point P in the intermediate coordinate system; α2 is the angle between the first joint coordinate system and the second joint coordinate system Z axis along the X2 axis of the second joint coordinate system; θ2 is the angle between the first joint coordinate system and the second joint coordinate system X axis along the Z1 axis of the first joint coordinate system; c means taking the cosine, s means taking the sine, l p is the distance between the connection center of the back second guide rail 204 and the back second joint module 203 and the geometric center of the third plane bearing 212;

[0061] The transformation relationship between two adjacent joint coordinate systems is expressed as:

[0062]

[0063] In the formula, i-1 P. i P is the position of point P in the i-1th, i-th joint coordinate system, α i Indicates around X i Axis from Z i-1 Axis rotation to Z i The angle of the axis; θ i represents the angle of the i-th joint, i.e. around Z i-1 Axis from X i-1 Axis rotation to X i The angle of the axis; a i Indicates that along X i Axis from Z i-1 Axis moves to Z i Axis distance, d i Indicates that along Z i-1 Axis from X i-1 Axis moves to X i The distance between the axes;

[0064] Therefore, the rotation and position transformation relationship from the fifth joint coordinate system to the base coordinate system is:

[0065]

[0066]

[0067] In the formula, is the rotation matrix from the fifth joint coordinate system to the base coordinate system; Indicates that the i-th joint coordinate system is respectively Z i and X i The axis is the rotation axis and the rotation angle is θ i , α i The rotation matrix of Y γ represents the rotation matrix between the intermediate coordinate system and the second joint coordinate system, 0 P.5 P is the position of point P in the base coordinate system and the fifth joint coordinate system, l h is the length of the upper arm;

[0068] In the rotation matrix There is no rotation transformation about angle θ2, and the rotation of angle θ2 represents the rotation of the parallelogram joint, which only affects the position calculation of point P.

[0069] Step 3: Calculate joint angles through inverse kinematics;

[0070] The angle between the base coordinate system and the joint coordinate system is converted into the joint angle through inverse kinematics:

[0071]

[0072] In the formula, 0 O5 is the position matrix of the origin of the fifth joint coordinate system relative to the base coordinate system. 0 O 2,3,4 is the position matrix of the origin of the second joint coordinate system to the fourth joint coordinate system relative to the base coordinate system;

[0073] in, 0 O5, and 0 O 2,3,4 are known quantities. θ1 and θ2 in formula (6) can be calculated from the spatial position vectors of the origins of joint coordinate systems 1 and 2 relative to the base coordinate system. Therefore, when θ1 and θ2 are known, θ3, θ4 and θ5 can be calculated according to formula (7).

[0074] Step 4: Conduct inverse dynamics modeling of the upper limb rehabilitation robot to obtain the spatial velocity and spatial force of the terminal joint, providing guidance for the motion planning and real-time control of each joint of the upper limb rehabilitation robot;

[0075] According to the Newton-Euler recursive dynamic equation, the dynamics of each joint of the upper limb rehabilitation robot is expressed as:

[0076]

[0077] In the formula, is the spatial velocity of the i-th and i-1-th joints, is the spatial acceleration of the i-th and i-1-th joints, is the joint axis vector of the ith joint, are the angular velocity and angular acceleration of the i-th joint respectively;

[0078] The feedforward torque is calculated by backward recursion and is expressed as:

[0079]

[0080] In the formula, is the spatial force of the i-th and i+1-th joints, is the spatial inertia matrix of the i-th joint, τ i is the torque of the ith joint, ⊙ represent spatial cross product and dot product respectively;

[0081] From formula (8), we can see that the spatial velocity of the end joint is expressed as:

[0082]

[0083] In the formula, is the spatial velocity of the terminal joint n, is the angular velocity of the ith joint, represents the Jacobian matrix of the end joint;

[0084] According to the principle of virtual work between joint space and task space, the relationship between the torque of the end joint and the spatial force (spatial force refers to the force exerted by the human body on the joint) is:

[0085]

[0086] In the formula, is the vector of the end joint torque, is the spatial force applied to the end joint, 03 is the 3×3 zero matrix, I3 is the 3×3 identity matrix, τ n is the scalar end joint torque.

[0087] In order to apply the desired force or impedance in the task space, the velocity of the interaction port (i.e., the point of contact between the human and the robot arm) needs to be calculated based on the spatial velocity of the link. Assuming that point P is attached to the end link (i.e., the forearm) and point O is the origin of the fifth joint coordinate system, the velocity of point P is:

[0088] 0 v P = 0 v O -OP× 0 ω (16)

[0089] Therefore, the spatial velocity of the end joint is expressed as:

[0090]

[0091] In the formula, 0 v P , 0 v O They are the linear velocities of points P and O respectively, OP is the position vector from point O to point P, 0ω is the angular velocity of the end joint;

[0092] The force or torque applied to the end joint is converted into spatial form by the following formula:

[0093]

[0094] In the formula, are the space vectors describing the external force and torque, respectively. F and M are the external force and torque applied to the end joint, respectively. 0 ω F is the angular velocity at the point of application of the external force, 0 ω M is the angular velocity at the point of action of the torque, 0 ρ F It is the position vector from the reference point (usually the center of mass of the connecting rod or a fixed point) to the point where the external force acts. is the zero vector.

[0095] Equation (18) describes the force and torque on the end joint in space, which provides guidance for calculating the expected force or impedance exerted by the upper limb rehabilitation robot on the human body, thereby obtaining the control force and direction, improving the effect and safety of rehabilitation training.

[0096] Any matters not described in the present invention are applicable to the prior art.

Claims

1. An upper limb rehabilitation robot based on a parallelogram joint, comprising a base, a back structure, a shoulder structure, an upper arm and a lower arm; characterized in that: The back structure includes a first back joint module, a back structural member, a second back joint module, a first back guide rail and a second back guide rail; the shoulder structure includes a first shoulder joint module, a first shoulder structural member, a second shoulder structural member, a second shoulder joint module, a third shoulder structural member and a third shoulder joint module; The first back joint module is located on the base, the axis of the first back joint module coincides with the sagittal axis of the human body, the upper end of the back structure is connected to the load end of the first back joint module, the second back joint module is connected to the lower end of the back structure, the load end of the second back joint module is fixedly connected to one end of the second back guide rail, and a plane bearing is slidably connected to the second back guide rail; one end of the first back guide rail is rotatably connected to the back structure, and a plane bearing is slidably connected to the first back guide rail; the connection center of the first back guide rail and the back structure, the connection center of the second back guide rail and the second back joint module, and the geometric centers of the two plane bearings correspond to the four vertices of the parallelogram, so the first back guide rail, the second back guide rail and the two plane bearings together constitute a parallelogram joint; The lower end of the first shoulder structural member is connected to the two plane bearings of the shoulder structure at the same time, the first shoulder joint module is connected to the upper end of the first shoulder structural member and the lower end of the second shoulder structural member at the same time, the second shoulder joint module is connected to the upper end of the second shoulder structural member and the upper end of the third shoulder structural member at the same time, the third shoulder joint module is connected to the lower end of the third shoulder structural member and the upper end of the upper arm at the same time, the axes of the first shoulder joint module, the second shoulder joint module and the third shoulder joint module intersect at one point, and the point is collinear with the center line of the two plane bearings of the shoulder structure.

2. The upper limb rehabilitation robot based on parallelogram joints according to claim 1, characterized in that: The upper arm comprises an upper arm first structural member, an upper arm second structural member, an upper arm joint module and an upper arm fixing member; The upper end of the first structural part of the upper arm is connected to the third joint module of the shoulder, the second structural part of the upper arm is inserted into the first structural part of the upper arm and the insertion depth is adjustable, and the upper arm fixing part is located on the inner side of the second structural part of the upper arm; The upper arm joint module is connected to the lower end of the second structural member of the upper arm and the upper end of the lower arm at the same time.

3. A dynamic modeling method for an upper limb rehabilitation robot based on a parallelogram joint according to claim 1 or 2, characterized in that: The following steps are involved: Step 1: Establish the base coordinate system and joint coordinate system; A base coordinate system O0-X0Y0Z0 is established with the intersection of the sagittal axis and the coronal axis of the human body as the origin, and the Z0 axis of the base coordinate system is collinear with the axis of the first back joint module and points to the first back joint module; Rotate the Z0 axis of the base coordinate system around the coronal axis of the human body by -90° to obtain the first joint coordinate system O1-X1Y1Z1; take the intersection of the axes of the first shoulder joint module, the second shoulder joint module and the third shoulder joint module as the origin to establish the second joint coordinate system O2-X2Y2Z2 to the fourth joint coordinate system O4-X4Y4Z4 respectively. The Z2 axis of the second joint coordinate system is collinear with the axis of the first shoulder joint module and points to the first shoulder joint module. The Z3 axis of the third joint coordinate system is collinear with the axis of the first shoulder joint module and points to the first shoulder joint module. The axis of the second joint module is collinear and points to the second joint module of the shoulder. The Z4 axis of the fourth joint coordinate system is collinear with the axis of the third joint module of the shoulder and points to the third joint module of the shoulder. The fifth joint coordinate system O5-X5Y5Z5 is established with the intersection of the long center line of the inner side of the upper arm and the axis of the upper arm joint module as the origin. The Z5 axis of the fifth joint coordinate system is collinear with the long center line of the inner side of the upper arm and points to the forearm. The Y2 axis of the second joint coordinate system is used as the rotation axis, and the rotation angle is γ to obtain the intermediate coordinate system O2 ’ -X2 ’ Y2 ’ Z2 ’ , Z2 of the intermediate coordinate system ’ The center lines of the shaft and the two plane bearings of the shoulder structure are collinear and point to the two plane bearings; Step 2: Use the standard DH parameter method to perform forward kinematic modeling on the upper limb rehabilitation robot, and obtain the rotation and position transformation relationship from the fifth joint coordinate system to the base coordinate system: In the formula, is the rotation matrix from the fifth joint coordinate system to the base coordinate system; Indicates that the i-th joint coordinate system is respectively Z i and X i The axis is the rotation axis and the rotation angle is θ i , α i The rotation matrix, i = 1, 2, 3, 4, 5; θ i is the angle of the i-th joint, α i Indicates around X i Axis from Z i-1 Axis rotation to Z i Angle of axis, Y γ represents the rotation matrix between the intermediate coordinate system and the second joint coordinate system, 0 P. 5 P is the position of any point P on the robot in the base coordinate system and the fifth joint coordinate system, l h is the length of the upper arm, l p It is the distance between the connection center of the back second guide rail and the back second joint module and the center of the plane bearing located on the second guide rail; Step 3: Solve the inverse kinematics according to the following formula to obtain the joint angles θ1~θ5; In the formula, 0 O5 is the position matrix of the origin of the fifth joint coordinate system relative to the base coordinate system. 0 O 2,3,4 is the position matrix of the origin of the second joint coordinate system to the fourth joint coordinate system relative to the base coordinate system; Step 4: Conduct inverse dynamics modeling of the upper limb rehabilitation robot to obtain the spatial velocity and spatial force of the terminal joint; Space velocity of the end joint It is expressed as: In the formula, 0 ω is the angular velocity of the end joint, 0 v O is the linear velocity of the origin O of the fifth joint coordinate system; Spatial force at the distal joint It is expressed as: In the formula, is a space vector describing external forces and torques, F and M are external forces and torques applied to the end joints, 0 ω F is the angular velocity at the point of application of the external force, 0 ω M is the angular velocity at the point of action of the torque, 0 ρ F is the position vector from the reference point to the point where the external force acts, is the zero vector.

4. The dynamic modeling method of the upper limb rehabilitation robot based on parallelogram joints according to claim 3 is characterized in that: In the first step, two adjacent frames of images are randomly intercepted during the movement of the upper limb rehabilitation robot. According to the position of any point P on the robot in the two frames of images, the transformation relationship between the second joint coordinate system and the first joint coordinate system is obtained as follows: In the formula, 1 P. 2 P is the position of point P in the first and second joint coordinate systems, 2′ P is the position of point P in the intermediate coordinate system; α2 is the angle between the first joint coordinate system and the second joint coordinate system Z axis along the X2 axis of the second joint coordinate system; θ2 is the angle between the first joint coordinate system and the second joint coordinate system X axis along the Z1 axis of the first joint coordinate system; c means taking the cosine, and s means taking the sine; The transformation relationship between two adjacent joint coordinate systems is expressed as: In the formula, i-1 P. i P is the position of point P in the i-1th, i-th joint coordinate system, a i Indicates that along X i Axis from Z i-1 Axis moves to Z i Axis distance, d i Indicates that along Z i-1 Axis from X i-1 Axis moves to X i Axis distance.

Images