Elimination modeling and force mapping control method for planar five-bar closed-chain rehabilitation robot

Abstract

The present application relates to a kind of plane five-pole closed-chain rehabilitation robot elimination modeling and force mapping control method, belong to rehabilitation robot control technical field.The method is aimed at the coupling of active joint and passive joint in plane five-pole closed-chain mechanism, control implementation difficulty under active joint coordinates and the problem that driving torque is difficult to accurately map as end force, establishes closed-chain position constraint equation, obtains velocity constraint relationship by derivation to constraint equation, under the nonsingular condition of passive joint constraint jacobian matrix, passive joint variable is explicitly eliminated, constraint consistent jacobian matrix is constructed, reduced dynamics equation under active joint coordinates is established, end force estimate is solved, and motor driving torque instruction is generated according to the end force estimate.The method is suitable for end impedance control, trajectory tracking control and rehabilitation training boost control of plane five-pole closed-chain rehabilitation robot.

Description

Technical Field

[0001] This invention belongs to the field of rehabilitation robot control technology, specifically relating to a method for elimination modeling and force mapping control of a planar five-bar closed-chain rehabilitation robot. Background Technology

[0002] Upper limb rehabilitation robots offer highly repeatable, adjustable-intensity, and quantifiable training methods, making them valuable for upper limb function recovery after nerve injury. Compared to wearable exoskeletons, end-effector traction upper limb rehabilitation robots are more compact, easier to operate, and better suited for in-plane guided training and task-oriented training.

[0003] Planar five-bar linkages are often used in upper limb rehabilitation training devices because they offer advantages such as easy centralized arrangement of drive components, high output stiffness, and relatively stable motion. However, planar five-bar linkages are typical closed-chain mechanisms, with significant coupling between their active and passive joints, which increases the difficulty of kinematic modeling, dynamic modeling, and end-effector force calculation.

[0004] In existing technologies, research on planar five-bar linkages mainly focuses on geometric kinematics analysis, ordinary Jacobian matrix analysis, workspace analysis, and trajectory tracking control. For rehabilitation robots, to achieve end-effector impedance control, assist control, and interaction force analysis, it is necessary to further establish the correspondence between driving torque and end-effector force. However, in the presence of closed-chain constraints, traditional full-coordinate dynamic models usually include constraint reaction terms, which is not conducive to direct control implementation in the active joint coordinate system.

[0005] Furthermore, although passive joint variables are not directly used as driving inputs, they affect the end effector motion and end effector force distribution through closed-chain constraints. Without explicit elimination of passive joint variables, it is difficult to establish a direct mapping relationship between active joint velocities and end effector Cartesian velocities, and it is also difficult to form a reduced dynamic model suitable for control implementation.

[0006] Therefore, it is necessary to propose a method for elimination modeling and force mapping control of a planar five-bar closed-chain rehabilitation robot to solve the problems in the existing technology of difficulty in achieving active joint coordinate control of closed-chain mechanisms, difficulty in accurately mapping driving torque into end force, and difficulty in directly generating end control quantities. Summary of the Invention

[0007] To address the shortcomings of existing technologies and solve the aforementioned problems, this invention provides an elimination modeling and force mapping control method for a planar five-bar closed-chain rehabilitation robot. This method establishes closed-chain position constraint equations, performs explicit elimination on passive joint variables, constructs a constraint-consistent Jacobian matrix from the active joint to the end effector in Cartesian space, and establishes reduced dynamic equations in the active joint coordinates. Then, it calculates the end effector force and outputs motor drive torque commands, thereby achieving end effector impedance control, trajectory tracking control, or rehabilitation training assistance control for the planar five-bar closed-chain rehabilitation robot.

[0008] The technical solution adopted by this invention to solve the technical problem is as follows:

[0009] This invention provides a method for elimination modeling and force mapping control of a planar five-bar closed-chain rehabilitation robot. The method includes: establishing a mechanism model; acquiring the states of active joints and establishing closed-chain position constraint equations; differentiating the position constraint equations to obtain velocity constraint relationships and performing explicit elimination on passive joint variables; constructing a constraint-consistent Jacobian matrix between the active joint velocities and the end effector Cartesian velocities; establishing reduced dynamic equations in the active joint coordinates; establishing a mapping relationship between driving torque and end effector force based on the principle of virtual work; and generating motor driving torque commands based on the estimated end effector force, reference trajectory, reference velocity, measured end effector position, and measured end effector velocity, and outputting these commands to the active driving joints.

[0010] The method includes the following steps:

[0011] Step 1: Establish the mechanism model:

[0012] The planar five-bar closed-chain rehabilitation robot is represented as a planar closed-chain mechanism consisting of the left branch ABP and the right branch DCP. A planar coordinate system is established, and points are defined. and points To actively rotate the secondary center, point and points For the passive rotation of the secondary center, point The end-effector is defined as the active joint variable. and Passive joint variables are and Define the link parameters as follows , , , and base distance .

[0013] Step 2: Acquire state variables and establish position constraint equations:

[0014] The active joint angles and active joint angular velocities of the planar five-bar closed-chain rehabilitation robot are collected, and the end points are solved based on the geometric relationships of the left branch (ABP) and the right branch (DCP). The positional expression is used to obtain the end position of the left branch. The end position of the right branch calculation Based on the coincidence of the endpoints Establish the position constraint equations for a planar five-bar closed chain mechanism. ,in It is a generalized coordinate system that includes both active and passive joint variables.

[0015] Step 3: Establish velocity constraint relationships and perform passive joint elimination:

[0016] For the position constraint equations in step 2 By performing time differentiation, the velocity constraint relationship between the active and passive joint velocities is obtained. The constraint Jacobian matrix is ​​decomposed into Jacobian matrix blocks corresponding to the active joint variables and Jacobian matrix blocks corresponding to the passive joint variables. When the constraint Jacobian matrix block corresponding to the passive joint variables is non-singular, the passive joint velocity is expressed as a function of the active joint velocity, thereby explicitly eliminating the passive joint velocity variable and constructing the elimination matrix. .

[0017] Step 4: Construct a constrained consistent Jacobian matrix:

[0018] Establish endpoints For all generalized coordinates The full coordinate Jacobian matrix Based on the elimination matrix obtained in step 3 Construct a constrained consistent Jacobian matrix between the active joint velocity and the end effector Cartesian velocity. .

[0019] Step 5: Establish the reduced kinetic equations:

[0020] The full-coordinate dynamic equations of the planar five-bar closed-chain rehabilitation robot are established using the Lagrange method. These equations include inertial terms, velocity-related terms, gravity terms, driving torque terms, and constraint reaction force terms caused by the closed-chain constraints. The elimination matrix from step 3 is then used... Projecting the full-coordinate dynamic equations and eliminating constraint reaction terms yields the reduced dynamic equations in the active joint coordinates, which include the equivalent inertia matrix. Equivalent speed related terms and equivalent gravity term .

[0021] Step 6: Establish the end force mapping relationship:

[0022] Based on the constraint-consistent Jacobian matrix in step 4 And the principle of virtual work, establishing the active joint driving torque and end point The mapping relationship between the forces is determined; the end force is directly obtained under static equilibrium conditions, and the estimated value of the end force is obtained after compensating for the velocity-related terms and gravity terms by combining the reduced dynamic equations in step 5 under dynamic motion conditions.

[0023] Step 7: Generate drive control quantities:

[0024] The estimated end effector force obtained in step 6, along with the preset reference trajectory, reference velocity, measured end position, and measured end velocity, is input into the task space controller. The task space controller then generates the corresponding motor drive torque command and outputs it to the left and right drive motors to achieve end impedance control, trajectory tracking control, or rehabilitation training assistance control for the planar five-bar closed-chain rehabilitation robot.

[0025] Compared with existing technologies, this invention has the following advantages: by explicitly eliminating passive joint variables, the kinematics and dynamics of the closed-loop mechanism are expressed in the active joint coordinates, reducing the modeling complexity of the closed-loop system; by constructing a constraint-consistent Jacobian matrix, a direct mapping relationship between the active joint velocity and the end effector Cartesian velocity is established; by reducing the projection of the full-coordinate dynamic equations, constraint reaction terms are eliminated, and a reduced dynamic equation suitable for control implementation is obtained; by combining the end effector force estimation with the task space controller, the motor drive torque command for the active joint can be directly generated, improving the feasibility and control accuracy of the end effector control of the closed-loop rehabilitation robot. Attached Figure Description

[0026] Figure 1 This is a schematic diagram of the overall structure of the planar five-bar closed-chain rehabilitation robot of the present invention.

[0027] Figure 2 This is a schematic diagram of the kinematic model of the planar five-bar closed chain mechanism of the present invention.

[0028] Figure 3 This is a schematic diagram of closed-chain constraint modeling in this invention.

[0029] Figure 4 This is a flowchart of the control method of the present invention.

[0030] Figure 5 This is a block diagram of the reduced dynamics and end force estimation of the present invention.

[0031] Figure 6 This is a block diagram of the control application of the present invention. Detailed Implementation

[0032] The present invention will now be described in detail with reference to the accompanying drawings.

[0033] like Figure 1 As shown, the planar five-bar closed-chain rehabilitation robot of this embodiment includes a base (1), a left drive motor (2), a right drive motor (3), a left branch (4), a right branch (5), an end effector (6), a forearm support (7), and a connecting component (8). The base (1) is used to support the overall structure. The left drive motor (2) and the right drive motor (3) provide active drive for the left and right branches, respectively. The left branch (4) and the right branch (5) close in the end effector output area to form a planar five-bar closed-chain mechanism. The end effector (6) is used to contact the user's upper limb end. The forearm support (7) is used to maintain the forearm posture during training. The connecting component (8) is used to realize the structural connection between the end effector output point and the human-machine interaction handle.

[0034] In one embodiment, the planar five-bar closed-chain rehabilitation robot is a symmetrical planar five-bar mechanism, with a left-right active drive center distance of [missing information]. The length of each branch can be set according to training requirements; in a specific embodiment, the length of each branch can be taken as... Handle relative to the mechanism output point setting The offset amount is used to improve the human-machine grip alignment.

[0035] like Figure 2 As shown, the planar five-bar closed-loop mechanism consists of a left branch ABP and a right branch DCP, with the two branches at their respective endpoints. The point is closed. Establish a plane coordinate system and define the point. and points Located at the fixed end of the base, at its midpoint With point The revolute joint at that point is an active revolute joint, and the active joint angles are respectively... and ;point and points The revolute joint at that point is a driven revolute joint, and the driven joint angles are respectively... and Define the length of the link in the left branch as... and The length of the link on the right branch is and ,point With point The distance between them is .

[0036] To facilitate the establishment of the positional relationships of the planar five-bar closed chain mechanism, a two-dimensional rotation matrix is ​​defined as shown in equation (1):

[0037] (1)

[0038] in, For the corner A two-dimensional rotation matrix; For any angle; for The cosine value; for The sine value.

[0039] The key points of the left and right branches are shown in equations (2) to (5), respectively:

[0040] (2)

[0041] in, For point The position vector; For point The position vector; For the active joint angle The constructed two-dimensional rotation matrix; For point The angle of rotation of the active joint; for to The length of the connecting rod.

[0042] (3)

[0043] in, The end point calculated from the left branch The position vector; For point The position vector; For passive joint angle The constructed two-dimensional rotation matrix; For point The angle of a passive joint; for to The length of the connecting rod.

[0044] (4)

[0045] in, For point The position vector; For point The position vector; For the active joint angle The constructed two-dimensional rotation matrix; For point The angle of rotation of the active joint; for to The length of the connecting rod.

[0046] (5)

[0047] in, The terminal point calculated from the right branch The position vector; For point The position vector; For passive joint angle The constructed two-dimensional rotation matrix; For point The angle of a passive joint; for to The length of the connecting rod.

[0048] like Figure 3 As shown, since both the left and right branches eventually reach the same endpoint... Therefore, the planar five-bar closed chain mechanism satisfies the following positional constraint relationship:

[0049] (6)

[0050] in, Let be the position constraint function of the planar five-bar closed chain mechanism; It is a fully generalized coordinate vector; The position vector of the end point of the left branch is calculated. The position vector of the end point of the right branch is calculated. It is a zero vector.

[0051] Taking the time derivative of equation (6), the velocity constraint relationship is obtained as shown in equation (7):

[0052] (7)

[0053] in, Position constraint function The partial derivative matrix with respect to active joint variables; Position constraint function The partial derivative matrix with respect to passive joint variables; This is the active joint velocity vector; This is the passive joint velocity vector.

[0054] when In the non-singular case, the passive joint velocity can be explicitly represented by the active joint velocity, as shown in equation (8):

[0055] (8)

[0056] in, This is the passive joint velocity vector; For matrix The inverse matrix; This is the partial derivative matrix of the position constraint function with respect to the active joint variables; This is the active joint velocity vector.

[0057] Further construct the elimination matrix so that the total generalized velocity satisfies equation (9):

[0058] (9)

[0059] in, It is the fully generalized velocity vector; The elimination matrix; This is the active joint velocity vector.

[0060] In terms of end-effector motion mapping, the end-effector point is established. For the full generalized coordinates, construct the full coordinate Jacobian matrix and the constrained consistent Jacobian matrix, as shown in equations (10) and (11):

[0061] (10)

[0062] in, To constrain a consistent Jacobian matrix; End point For all generalized coordinates The full-coordinate Jacobian matrix; This is the elimination matrix.

[0063] (11)

[0064] in, End point Cartesian velocity vector; To constrain a consistent Jacobian matrix; This is the active joint velocity vector.

[0065] like Figure 4 As shown, the control method flow of the present invention includes: establishing a mechanism model, acquiring active joint angles and active joint angular velocities, establishing closed-chain position constraint equations, obtaining velocity constraint relationships by differentiation, eliminating passive joint variables and constructing elimination matrices, constructing constraint-consistent Jacobian matrices, establishing reduced dynamic equations, obtaining end-effector force estimates, and generating motor drive torque commands.

[0066] In terms of dynamic modeling, the full-coordinate dynamic equations of the planar five-bar closed-chain rehabilitation robot are first established, as shown in equation (12):

[0067] (12)

[0068] in, The inertia matrix is ​​the matrix in all coordinates. It is the generalized acceleration vector; For speed-related terms; This is the term related to gravity. To drive the input mapping matrix; This is the active joint drive torque vector; To constrain the Jacobian matrix; for The transpose of the matrix; is a Lagrange multiplier vector.

[0069] By projecting the elimination matrix from step 3 onto equation (12), the equivalent inertia matrix in the active joint coordinates is obtained, as shown in equation (13):

[0070] (13)

[0071] in, This is the equivalent inertia matrix in active joint coordinates; Elimination matrix The transpose of the matrix; The inertia matrix is ​​the matrix in all coordinates. This is the elimination matrix.

[0072] Furthermore, the reduced dynamic equations in the active joint coordinates can be obtained, as shown in equation (14):

[0073] (14)

[0074] in, The equivalent inertia matrix; This is the active joint acceleration vector; This refers to the equivalent velocity-related terms in active joint coordinates. This is the equivalent gravity term in active joint coordinates; This is the active joint drive torque vector.

[0075] like Figure 5 As shown, the inputs to the reduced dynamics and end-effector force estimation process include active joint angle, active joint angular velocity, link parameters, and driving torque. The intermediate processes include constraint solving, elimination matrix calculation, constraint-consistent Jacobian matrix construction, and reduced dynamics compensation. The outputs include end-effector force estimates, equivalent inertia matrix, equivalent velocity-related terms, and equivalent gravity terms.

[0076] Regarding end-effector force mapping, the relationship between the active joint drive torque and the end-effector point is established based on the principle of virtual work. The relationship between the forces is shown in equation (15):

[0077] (15)

[0078] in, This is the active joint drive torque vector; To constrain the consistent Jacobian matrix The transpose of the matrix; End point The force vector at that point.

[0079] In one implementation, when In the non-singular case, the estimated end effect force can be calculated based on the active joint drive torque; in another embodiment, the active joint drive torque command can also be derived from the desired end effect force for end impedance control and assist control.

[0080] like Figure 6 As shown, in the control application, the reference trajectory, reference speed, measured end position and measured end speed are input into the task space controller, and the estimated end force obtained in step 6 is input into the end force mapping module and torque compensation module. After control calculation, the motor drive torque is output to drive the active joint movement of the planar five-bar closed chain rehabilitation robot.

[0081] In one embodiment, the task space controller is an end-effector impedance controller; in another embodiment, the task space controller is a trajectory tracking controller or a rehabilitation training assist controller, and may be combined with a path constraint strategy to restrict the movement of the robot's end-effector output point within a predetermined training area.

[0082] This invention is not limited to the embodiments described above. Various modifications and improvements can be made by those skilled in the art without departing from the principles of this invention, and these modifications and improvements should also be considered within the scope of protection of this invention.

Claims

1. A method for elimination modeling and force mapping control of a planar five-bar closed-chain rehabilitation robot, characterized in that, This control method, applied to a planar five-bar closed-chain rehabilitation robot, is used to determine the end effector force based on the state of the robot's active joints and output a drive control quantity. The control method includes the following steps: Step 1: Establish the mechanism model: The planar five-bar closed-chain rehabilitation robot is represented as a planar closed-chain mechanism consisting of the left branch ABP and the right branch DCP. A planar coordinate system is established, and points are defined. and points To actively rotate the secondary center, point and points The passive rotation joint center is defined, and point P is the end-effector output point; the active joint variable is defined as follows. and Passive joint variables are and Define the link parameters as follows , , , and base distance ; Step 2: Acquire state variables and establish position constraint equations: The active joint angles and active joint angular velocities of the planar five-bar closed-chain rehabilitation robot are collected, and the end points are solved based on the geometric relationships of the left branch (ABP) and the right branch (DCP). The positional expression is used to obtain the end position of the left branch. The end position of the right branch calculation Based on the coincidence of the endpoints Establish the position constraint equations for a planar five-bar closed chain mechanism. ,in The coordinates are generalized and include both active and passive joint variables. Step 3: Establish velocity constraint relationships and perform passive joint elimination: The position constraint equations established in step 2 By performing time differentiation, the velocity constraint relationship between the active and passive joint velocities is obtained. The constraint Jacobian matrix is ​​decomposed into Jacobian matrix blocks corresponding to the active joint variables and Jacobian matrix blocks corresponding to the passive joint variables. When the constraint Jacobian matrix block corresponding to the passive joint variables is non-singular, the passive joint velocity is expressed as a function of the active joint velocity, and an elimination matrix is ​​constructed. This is to complete the explicit elimination of passive joint variables; Step 4: Construct a constrained consistent Jacobian matrix: Establish endpoints For all generalized coordinates The full coordinate Jacobian matrix Based on the elimination matrix obtained in step 3 Construct a constrained consistent Jacobian matrix between the active joint velocity and the end effector Cartesian velocity. ; Step 5: Establish the reduced kinetic equations: The full-coordinate dynamic equations of the planar five-bar closed-chain rehabilitation robot are established using the Lagrange method. These equations include inertial terms, velocity-related terms, gravity terms, driving torque terms, and constraint reaction force terms caused by the closed-chain constraints. The elimination matrix from step 3 is then used... Projecting the full-coordinate dynamic equations and eliminating constraint reaction terms yields the reduced dynamic equations in the active joint coordinates, which include the equivalent inertia matrix. Equivalent speed related terms and equivalent gravity term ; Step 6: Calculate the final force: Based on the constraint-consistent Jacobian matrix in step 4 And the principle of virtual work, establishing the active joint driving torque and end point The mapping relationship between the forces is determined. Under static equilibrium conditions, the end force is directly obtained. Under dynamic conditions, the estimated end force is obtained by compensating for the velocity-related terms and gravity terms by combining the reduced dynamic equations in step 5. Step 7: Generate drive control quantities: The estimated end force obtained in step 6, along with the preset reference trajectory, reference speed, measured end position, and measured end speed, is input into the task space controller to generate the corresponding motor drive torque command, which is then output to the left and right drive motors to achieve end impedance control, trajectory tracking control, or rehabilitation training assistance control.

2. The elimination modeling and force mapping control method for the planar five-bar closed-chain rehabilitation robot according to claim 1, characterized in that: The planar five-bar closed-chain rehabilitation robot described in step 1 is a planar five-bar closed-chain mechanism consisting of two active revolute joints, two passive revolute joints, and two left and right branches. The two left and right branches are the ABP branch and the DCP branch, respectively, and they are located at the end output points. The area is closed.

3. The elimination modeling and force mapping control method for the planar five-bar closed-chain rehabilitation robot according to claim 1, characterized in that: The position constraint equations described in step 2 are established by making the position expressions of the left and right branches equal. They are used to describe the geometric closure relationship of the planar five-bar closed chain mechanism and to determine the constraint relationship between passive and active joint variables.

4. The elimination modeling and force mapping control method for the planar five-bar closed-chain rehabilitation robot according to claim 1, characterized in that: The passive joint elimination described in step 3 satisfies the following relationship: （1）； when Passive joint velocity in non-singular cases Represented as: （2）； And thus construct the elimination matrix Make the generalized velocity From active joint velocity express.

5. The elimination modeling and force mapping control method for the planar five-bar closed-chain rehabilitation robot according to claim 1, characterized in that: The constraint-consistent Jacobian matrix mentioned in step 4 From the full coordinate Jacobian matrix elimination matrix The product obtained by multiplication is used to map the active joint side velocity to the end point. The Cartesian velocity is used for end-effector motion analysis, singularity determination, and subsequent end-effector force mapping solution.

6. The elimination modeling and force mapping control method for the planar five-bar closed-chain rehabilitation robot according to claim 1, characterized in that: The reduced dynamic equations described in step 5 are obtained by performing null space projection or equivalent projection on the full coordinate dynamic equations, and their equivalent inertia matrix is... Equivalent speed related terms and equivalent gravity term Each by elimination matrix The inertial, velocity-related, and gravity terms in the full-coordinate dynamic equations are obtained by mapping.

7. The elimination modeling and force mapping control method for a planar five-bar closed-chain rehabilitation robot according to claim 1, characterized in that: The mapping relationship between the driving torque and the end force described in step 6 satisfies ,in For active joint drive torque, End point The force acting at the point; When in a dynamic state, the end force estimate is established after compensating for velocity-related terms and gravity terms based on the reduced dynamic equations.

8. The elimination modeling and force mapping control method for a planar five-bar closed-loop rehabilitation robot according to claim 1, characterized in that: In step 7, the task space controller generates a motor drive torque command based on the estimated end effector force, reference trajectory, reference velocity, measured end effector position, and measured end effector velocity, and outputs the motor drive torque command to the active drive joint of the planar five-bar closed-chain rehabilitation robot.

9. The elimination modeling and force mapping control method for a planar five-bar closed-chain rehabilitation robot according to claim 1, characterized in that: The task space controller mentioned in step 7 is an end impedance controller, a trajectory tracking controller, or a rehabilitation training assist controller.

Images