Method, system, device and medium for determining motion state based on inverse kinematics of human upper limbs

Abstract

The present application discloses a method, system, device and medium for determining the motion state of a human upper limb based on inverse kinematics, which relates to the field of robot kinematic control. The method comprises obtaining the upper limb workspace of a human upper limb; performing discrete sampling based on the Monte Carlo sampling method according to the upper limb workspace to determine an initial discrete point set; adjusting the sampling density of sensitive areas, boundary areas and singular areas in the initial discrete point set to obtain a final discrete point set; determining the joint angle set corresponding to each discrete point and constructing a joint angle database; retrieving the joint angle database based on the kd-tree method to determine the initial joint angle; and performing local optimization based on the gradient descent method to determine the joint angle corresponding to each end position; and then determining the motion state of the human upper limb; the present application can quickly and accurately determine the joint angle corresponding to each end position of the human upper limb, and then determine the current motion state of the human upper limb.

Description

Technical Field

[0001] The present application relates to the field of robot kinematic control, and in particular to a method, system, device and medium for determining the motion state based on inverse kinematics of the human upper limb. Background Art

[0002] The kinematic analysis of the human upper limb has important research value in the fields of robot-assisted rehabilitation, prosthetic control, and human-computer interaction. Due to the complexity of the upper limb joint structure and the strong coupling effect between the joints, the kinematic solution becomes a high-dimensional nonlinear problem. Among them, inverse kinematics solution is the basis of trajectory planning and motion control, aiming to determine the joint angle combination that meets physiological constraints based on the desired position of the end effector. However, due to the redundant degrees of freedom of the upper limb, there are usually multiple solutions in the inverse kinematics solution process, and the motion characteristics of different solutions may vary greatly. Therefore, how to select the optimal solution among the feasible solutions becomes a key issue in the inverse kinematics solution of the human upper limb.

[0003] Currently, researchers mainly use analytical methods, numerical iterative methods, and optimization algorithms to solve inverse kinematics problems. Analytical methods can directly solve closed-form solutions, but they are only applicable to robots with specific structures and are difficult to generalize to upper limb models with redundant degrees of freedom. Numerical iterative methods (such as the Newton-Raphson method) can handle general inverse kinematics problems, but they rely on the selection of initial values, are prone to falling into local optimality, and have high computational costs. Optimization algorithms (such as genetic algorithms and particle swarm optimization) can search for optimal solutions within a large range, but have high computational complexity and are difficult to meet real-time requirements. In addition, traditional methods usually need to calculate the inverse solution from scratch during the solution process, and lack an efficient initialization strategy, resulting in high computational redundancy and slow convergence.

[0004] Therefore, based on the above problems, there is an urgent need to provide a new motion state determination method based on inverse kinematics of the human upper limb, so as to quickly and accurately determine the joint angle corresponding to each end posture of the human upper limb, and then determine the current motion state of the human upper limb, thereby improving the accuracy and efficiency of the robot's control of the human body and improving the training effect of assisted upper limb rehabilitation training. Summary of the Invention

[0005] The purpose of this application is to provide a method, system, device and medium for determining the motion state of the human upper limb based on inverse kinematics, which can quickly and accurately determine the joint angle corresponding to each end posture of the human upper limb, and then determine the current motion state of the human upper limb, thereby improving the accuracy and efficiency of the robot's control of the human body and improving the training effect of assisted upper limb rehabilitation training.

[0006] To achieve the above objectives, this application provides the following solutions:

[0007] In a first aspect, the present application provides a method for determining a motion state of a human upper limb based on inverse kinematics, the method comprising:

[0008] Obtaining the upper limb working space of the human upper limb;

[0009] According to the upper limb workspace, discrete sampling is performed based on the Monte Carlo sampling method to determine an initial discrete point set;

[0010] The sampling density of the sensitive area, boundary area and singular area in the initial discrete point set is adjusted to obtain a final discrete point set; the sensitive area is an area where the sensitivity index corresponding to the discrete point is greater than a preset sensitivity threshold; the boundary area is an area where the distance between the discrete point and the upper limb workspace boundary is less than a preset distance threshold; the singular area is an area where the singularity index corresponding to the discrete point is greater than a preset singularity threshold; the sensitivity index is the Jacobian matrix condition number of the joint angle corresponding to the discrete point; the singularity index is the same as the sensitivity index;

[0011] Determining a joint angle set corresponding to each discrete point in the final discrete point set; and constructing a joint angle database based on the final discrete point set and the corresponding joint angle set;

[0012] According to the upper limb rehabilitation training trajectory, the joint angle database is searched based on the kd-tree method to determine the initial joint angle; the upper limb rehabilitation training trajectory includes the end position at different times; the end position includes: end position and posture information;

[0013] Performing local optimization on the initial joint angle based on the gradient descent method to determine the joint angle corresponding to each end position;

[0014] The motion state of the human upper limb is determined based on each end position and the corresponding joint angle.

[0015] Optionally, the process of determining the upper limb workspace includes:

[0016] Based on the human upper limb kinematic model, determine the joint angle vector;

[0017] According to the joint angle vector, a forward kinematics model is constructed based on the Denavit-Hartenberg parameter method;

[0018] The upper limb working space is determined based on the forward kinematics model.

[0019] Optionally, the process of determining the sensitive area includes:

[0020] Using the formula and formula determining the sensitive area;

[0021] in, is the joint angle corresponding to the discrete point in the initial discrete point set, is the Jacobian matrix of joint angle to end pose, is the condition number of the Jacobian matrix, is the preset sensitivity threshold, is the maximum singular value of the Jacobian matrix, is the minimum singular value of the Jacobian matrix.

[0022] Optionally, constructing a joint angle database based on the final discrete point set and the corresponding joint angle set specifically includes:

[0023] Eliminate the joint angles in the joint angle set based on the positive kinematics function to obtain a set of eliminated joint angles;

[0024] A joint angle database is constructed based on the final discrete point set and the corresponding joint angle set after elimination.

[0025] Optionally, searching the joint angle database based on the kd-tree method according to the upper limb rehabilitation training trajectory to determine the initial joint angle specifically includes:

[0026] Filtering the joint angle database based on an angle range query mechanism to obtain a joint angle database subset;

[0027] According to the upper limb rehabilitation training trajectory, a subset of the joint angle database is searched based on the kd-tree method;

[0028] According to the search results, determine the Euclidean distance between each discrete point and the current end position;

[0029] The joint angle corresponding to the discrete point with the smallest Euclidean distance is taken as the initial joint angle.

[0030] Optionally, the process of determining the Euclidean distance includes:

[0031] Using the formula Determine the Euclidean distance between each discrete point and the current end position;

[0032] in, For the Frame end position The joint angle database subset j discrete points The Euclidean distance of For the The frame end position is xThe coordinates on the axis, For discrete points exist x The coordinates on the axis, For the The frame end position is y The coordinates on the axis, For discrete points exist y The coordinates on the axis, For the The frame end position is z The coordinates on the axis, For discrete points exist z Coordinates on the axis.

[0033] Optionally, the locally optimizing the initial joint angle based on the gradient descent method to determine the joint angle corresponding to each end position specifically includes:

[0034] Using the formula Determine the objective function ;in, For joint angle vectors Each component of imposes a different error penalty, is the position error weight, is the end position determined by the forward kinematics model, is the target end position, is the angle continuity error weight, is the frame number, is the total number of joints, For the k The joint angles of each frame, For the -1 frame corresponding to each joint angle, is the angle normalization function, is the joint angular velocity error weight, is the time rate of change of the joint angle;

[0035] Using the formula Perform local optimization on the initial joint angles; For the The joint angle corresponding to the end pose of the frame, is the initial joint angle, is the learning rate, () is used to determine the gradients of each component of the joint angle using the finite difference method.

[0036] In a second aspect, the present application provides a motion state determination system based on inverse kinematics of a human upper limb, the motion state determination system based on inverse kinematics of a human upper limb comprising:

[0037] An upper limb workspace acquisition module is used to obtain the upper limb workspace of the human upper limb;

[0038] An initial discrete point set determination module is used to perform discrete sampling based on the upper limb workspace and the Monte Carlo sampling method to determine an initial discrete point set;

[0039] A final discrete point set determination module is configured to adjust the sampling density of the sensitive area, boundary area, and singular area in the initial discrete point set to obtain a final discrete point set; the sensitive area is an area where the sensitivity index corresponding to the discrete point is greater than a preset sensitivity threshold; the boundary area is an area where the distance between the discrete point and the boundary of the upper limb workspace is less than a preset distance threshold; the singular area is an area where the singularity index corresponding to the discrete point is greater than a preset singularity threshold; the sensitivity index is the Jacobian matrix condition number of the joint angle corresponding to the discrete point; and the singularity index is the same as the sensitivity index;

[0040] a joint angle database determination module, configured to determine a joint angle set corresponding to each discrete point in the final discrete point set; and construct a joint angle database based on the final discrete point set and the corresponding joint angle set;

[0041] An initial joint angle determination module is configured to determine the initial joint angle by searching the joint angle database based on the kd-tree method according to the upper limb rehabilitation training trajectory; the upper limb rehabilitation training trajectory includes the end position at different times; the end position includes: end position and posture information;

[0042] a joint angle determination module, configured to locally optimize the initial joint angle based on a gradient descent method to determine the joint angle corresponding to each end position;

[0043] The motion state determination module is used to determine the motion state of the human upper limb based on each end position and the corresponding joint angle.

[0044] In a third aspect, the present application provides a computer device comprising: a memory, a processor, and a computer program stored on the memory and runnable on the processor, wherein the processor executes the computer program to implement the steps of any one of the above-mentioned methods for determining the motion state based on inverse kinematics of the human upper limb.

[0045] In a fourth aspect, the present application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of any one of the above-mentioned methods for determining the motion state based on inverse kinematics of the human upper limb.

[0046] According to the specific embodiments provided in this application, this application has the following technical effects:

[0047] The present application provides a method, system, device and medium for determining the motion state of the human upper limb based on inverse kinematics. The method uses Monte Carlo sampling combined with sensitivity indicators and singularity indicators to dynamically adjust the sampling density, add sampling points in sensitive areas, boundary areas and singular areas, determine the final discrete point set, and then construct a joint angle database. The joint angle database is quickly retrieved through the kd-tree method to improve the efficiency of determining the initial joint angle. The initial joint angle is locally optimized through the gradient descent method to ensure the accuracy and stability of the joint angle determination and meet the real-time requirements. The present application combines the kd-tree method and local optimization to achieve accurate and rapid determination of the motion state of the redundant structure of the upper limb while ensuring computational efficiency, providing a more reliable solution for trajectory planning and motion control, thereby improving the accuracy and efficiency of the robot's control of the human body and improving the training effect of assisted upper limb rehabilitation training. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0049] Figure 1 This is a flow chart of a method for determining the motion state of a human upper limb based on inverse kinematics in one embodiment of the present application. DETAILED DESCRIPTION

[0050] The following will be combined with the drawings in the embodiments of this application to clearly and completely describe the technical solutions in the embodiments of this application. Obviously, the embodiments described are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0051] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below with reference to the accompanying drawings and specific implementation methods.

[0052] In an exemplary embodiment, Figure 1As shown, a method for determining the motion state of a human upper limb based on inverse kinematics is provided, including the following S1-S7. Among them, S1-S4 are all performed offline and can be generally summarized as the offline stage; S5-S7 are performed online and can be generally summarized as the online stage; S1-S7 are specifically:

[0053] S1: Obtain the upper limb working space of the human upper limb.

[0054] S1 specifically includes:

[0055] S11: Determine the joint angle vector based on the human upper limb kinematic model.

[0056] Joint angle vector It is a vector consisting of the joint angles of all joints in the human upper limb kinematic model, usually expressed in radians or degrees, and is defined as follows:

[0057] .

[0058] in, is the total number of joint angles, is the joint angle vector Component, that is, The joint angles of the joints, Number the joints, .in, To control the important parameters of single joint motion, all Comprehensively determine the terminal posture, The following constraints are met to ensure that the joint angle vectors meet the motion limitations of the human anatomy:

[0059] .

[0060] in, For the The minimum joint angle of each joint, For the The maximum joint angle of each joint, Indicates the The physically feasible angle range of each joint.

[0061] S12: Based on the joint angle vectors and the Denavit-Hartenberg parameter method, a forward kinematics model is constructed.

[0062] The forward kinematics model is used to map the joint angle vector to the homogeneous transformation matrix of the end pose to determine the end pose under a certain joint angle configuration. The calculation formula is as follows:

[0063] 。

[0064] in, is the homogeneous transformation matrix of the end pose, is the positive kinematic function.

[0065] S13: Determine the upper limb workspace based on the forward kinematics model.

[0066] The forward kinematics model provides accurate theoretical models and experimental data for the subsequent construction of the joint angle database.

[0067] S2: Based on the upper limb workspace, discrete sampling is performed based on the Monte Carlo sampling method to determine the initial discrete point set.

[0068] The upper limb workspace is sampled uniformly using the Monte Carlo sampling method to obtain the initial discrete point set. , the discrete points are the target end position, including the target end position and target posture information, where, For the discrete points, is the discrete point number, is the total number of discrete points sampled uniformly.

[0069] S3: Adjust the sampling density of the sensitive areas, boundary areas and singular areas in the initial discrete point set to obtain the final discrete point set.

[0070] During motion state determination, some joint configurations may lead to singularities, degenerating the Jacobian matrix and thus affecting the stability and feasibility of the calculated joint angles. Furthermore, in certain sensitive, boundary, and singular regions, joint angle determination becomes significantly more difficult, and conventional numerical methods are prone to convergence difficulties or discontinuous solutions. Therefore, to improve the accuracy and stability of joint angle determination, a rational optimization strategy is employed, taking into account the reachability distribution characteristics of the upper limb workspace. This ensures that the joint angle determination process remains stable within complex spaces and strikes a balance between computational efficiency and determination quality.

[0071] S3 specifically includes:

[0072] S31: Adjust the sampling density of sensitive areas.

[0073] In order to capture local kinematic changes, the joint angle of each discrete point in the initial discrete point set is determined, and the Jacobian matrix of the joint angle to the end pose is determined. The condition number of the Jacobian matrix is ​​used as a sensitivity index. The calculation formula of the sensitivity index is as follows:

[0074] .

[0075] in, is the joint angle corresponding to the discrete point in the initial discrete point set, is the Jacobian matrix of joint angle to end pose, is the condition number of the Jacobian matrix, is the maximum singular value of the Jacobian matrix, is the minimum singular value of the Jacobian matrix.

[0076] In an exemplary embodiment, a sensitivity threshold is set (For example, take 10). When the sensitivity index of a discrete point is greater than the sensitivity threshold, the neighborhood of the discrete point is the sensitive area. The judgment formula is as follows:

[0077] .

[0078] If the area is a sensitive area, it means that the joint angle in the area is sensitive to changes, and refined sampling is required in the area. During the refined sampling process, the sampling interval during the initial uniform sampling is reduced to generate more discrete points. The reduction formula is as follows:

[0079] .

[0080] in, is the initial sampling spacing when using the Monte Carlo sampling method for preliminary uniform sampling, It is the sensitive sampling interval when performing refined sampling on sensitive areas.

[0081] In an exemplary embodiment, the sensitive sampling interval is set to half of the initial sampling interval, and the discrete point density is the original times, among which is the spatial dimension, usually .

[0082] S32: Adjust the sampling density of the boundary area.

[0083] Discrete points at the boundary of the upper limb workspace often have drastic changes in kinematic characteristics due to physical limitations. The area where the discrete points are located is defined as the boundary area. The oversampling strategy is used to adjust the sampling density in the boundary area to refine the sampling. The specific formula is:

[0084] .

[0085] in, Boundary sampling spacing when oversampling the boundary area ， is the magnification factor of the boundary area, (like ).

[0086] S31: Adjust the sampling density of the singular area.

[0087] Similarly, when the singularity index corresponding to a discrete point in the initial discrete point set is greater than a preset singularity threshold, the region where the discrete point is located is defined as a singular region, and the sampling density of the singular region is adjusted using an oversampling strategy. In an exemplary embodiment, the singularity index is the same as the sensitivity index. The formula for oversampling the singular region is:

[0088] .

[0089] in, is the singular sampling spacing when refining the sampling of the singular region, is the magnification factor of the singular region.

[0090] After adjusting the sampling density of sensitive areas, boundary areas and singular areas in the initial discrete point set, the final discrete point set is obtained. ,in, The final total number of discrete points after adjusting the sampling density.

[0091] Denser sampling of sensitive areas, boundary areas, and singular areas can improve the accuracy and compactness of subsequent joint angle database construction.

[0092] S4: Determine the joint angle set corresponding to each discrete point in the final discrete point set; and construct a joint angle database based on the final discrete point set and the corresponding joint angle set.

[0093] S4 specifically includes:

[0094] S41: Determine a joint angle set corresponding to each discrete point in the final discrete point set.

[0095] The joint angle corresponding to each discrete point in the final discrete point set is calculated using the Jacobian matrix iteration method to obtain the joint angle set of each discrete point. ,in, For the discrete points The corresponding joint angles, For the discrete points The number of corresponding joint angles, For the discrete points The corresponding joint angle number.

[0096] S42: Eliminate the joint angles in the joint angle set based on the positive kinematics function to obtain a eliminated joint angle set.

[0097] For the joint angle set corresponding to each discrete point, the joint angles in the joint angle set are eliminated based on the positive kinematic function. The elimination process is:

[0098] Based on positive kinematics function Determine the end position corresponding to each joint angle , the calculation formula is as follows:

[0099] .

[0100] The end position Target end position Compare and determine the position error , the calculation formula is as follows:

[0101] .

[0102] in, is the norm of the vector.

[0103] The position error is compared with the preset position error threshold to determine whether the position error is greater than the preset position error threshold. If so, it means that the joint angle does not meet the accuracy requirement and the joint angle is eliminated. Otherwise, the joint angle is retained.

[0104] After removing the joint angles in the joint angle set, a removed joint angle set is obtained.

[0105] S43: Construct a joint angle database based on the final discrete point set and the corresponding joint angle set after elimination.

[0106] S5: According to the upper limb rehabilitation training trajectory, the joint angle database is searched based on the kd-tree method to determine the initial joint angle.

[0107] S5 specifically includes:

[0108] S51: Filter the joint angle database based on the angle range query mechanism to obtain a subset of the joint angle database.

[0109] In this application, the angle range query mechanism means obtaining the range of joint motion of each joint at the current stage based on the doctor's targeted assessment results of the current patient's upper limb joint motion range. The joint angle database is screened according to the joint motion range to filter out invalid joint angle combinations in the joint angle database. The angle range query mechanism can achieve personalized screening without rebuilding the joint angle database to obtain a subset of the joint angle database.

[0110] Traditional sampling spaces are mostly full-joint spaces, while most upper limb rehabilitation training movements have highly "restricted" activity areas. For example, the elbow joint can only flex and extend within a certain range; the shoulder joint only allows rotation in a specific direction; and the same movement has different amplitudes at different stages of rehabilitation. This application, based on the joint angle database, conducts personalized screening according to the different stages of different patients, breaking through the limitations of traditional sampling space. It is constructed according to the "activity domain" set at the rehabilitation stage, supports "progressive rehabilitation" (dynamic expansion of joint range of motion to match rehabilitation progress), significantly reduces data redundancy, provides a reliable physical basis for the subsequent determination of joint angles, and improves the accuracy and stability of joint angle calculations. This application restricts the search space from the perspective of "human functional science", which is a "medical constraint prior" that is not available in traditional robot joint angle determination methods.

[0111] S52: According to the upper limb rehabilitation training trajectory, a subset of the joint angle database is retrieved based on the kd-tree method.

[0112] Collect a segment of upper limb rehabilitation training trajectory of human upper limb in real time, recorded as The upper limb rehabilitation training trajectory includes the end postures at different times, among which, For the The end pose of the frame, each end pose includes the end position and posture information, is the number of data frames, The frame number.

[0113] Extract the end position of each frame in the upper limb rehabilitation training trajectory and record it as , discretize the upper limb rehabilitation training trajectory to form the end position set Discretizing the continuous upper limb rehabilitation training trajectory into multiple end positions makes it possible to subsequently match the initial joint angles using a subset of the joint angle database, providing a specific target for local optimization.

[0114] For each end position ,The kd-tree method is used to perform nearest neighbor search on the subset of joint angle database constructed in the offline stage.

[0115] S53: Determine the Euclidean distance between each discrete point and the current end position according to the search result.

[0116] The Euclidean distance calculation formula is as follows:

[0117] .

[0118] in, For the Frame end position The joint angle database subset j discrete points The Euclidean distance of For the The frame end position is x The coordinates on the axis, For discrete points exist x The coordinates on the axis, For the The frame end position is y The coordinates on the axis, For discrete points exist y The coordinates on the axis, For the The frame end position is z The coordinates on the axis, For discrete points exist z Coordinates on the axis.

[0119] S54: The joint angle corresponding to the discrete point with the smallest Euclidean distance is used as the initial joint angle.

[0120] Determine the discrete points with the smallest Euclidean distance , and extract the corresponding joint angle As the initial joint angles for the current frame.

[0121] Through efficient nearest neighbor search, the initial joint angle with a high degree of matching with the end position is quickly obtained, which reduces the computational burden of local optimization and improves the accuracy of joint angle determination.

[0122] S6: Perform local optimization of the initial joint angle based on the gradient descent method to determine the joint angle corresponding to each end position.

[0123] Initial joint angle matched to the end position As the starting point, construct the objective function :

[0124] .

[0125] in, For joint angle vectors Each component of imposes a different error penalty, is the position error weight, is the end position determined by the forward kinematics model, is the target end position, is the angle continuity error weight, is the frame number, is the total number of joints, For the k The joint angles of each frame, For the -1 frame corresponding to each joint angle, is the angle normalization function, is the joint angular velocity error weight, is the time rate of change of the joint angle.

[0126] The angle normalization function is defined as:

[0127] .

[0128] in, is the target joint angle to be compared, is the reference joint angle, ( ) is a modular operation, which means that the number Divide by a number The remainder obtained after , the result is usually in the interval In this application, it means Pan , and then use the modulus operation to limit the result to , and finally subtract To map the results to interval, which ensures that the calculated angle difference will not have discontinuity problems caused by angle range jumps.

[0129] The gradient descent method is used to locally optimize the initial joint angle, and the update formula is:

[0130] .

[0131] in, For the k The joint angle corresponding to the end pose of the frame, is the initial joint angle, is the learning rate, () is used to determine the gradients of each component of the joint angle using the finite difference method.

[0132] Among them, the joint angle The gradient of the components The calculation formula is as follows:

[0133] .

[0134] in, Represents the objective function, that is, the joint angle vector Each component of imposes a different error penalty, Represents the first joint angle vector Component, that is, The angle of the joint. Indicates that Middle Add a small amount The objective function value is calculated later. Represents a small positive perturbation used in finite difference calculations.

[0135] The method presented in this application obtains an optimal initial joint angle through a joint angle database search, allowing local optimization to converge quickly around this initial joint angle. Local optimization fine-tunes the initial joint angle, compensating for discretization errors and ensuring that the optimized joint angle meets the requirements for continuity and smoothness, thus satisfying real-time control requirements with high precision.

[0136] S7: Determine the motion state of the human upper limb based on each end position and the corresponding joint angle.

[0137] The joint angles obtained from local optimization for each frame are output in chronological order, forming a complete time series of upper limb joint angle data to determine the motion state of the human upper limb. This continuous sequence of joint angles ensures smooth and accurate input for the real-time control system, completing a complete closed-loop process from raw data acquisition and offline database construction to online initial joint angle matching and local optimization, facilitating subsequent use in rehabilitation robot motion control and real-time feedback systems.

[0138] This application adopts an adaptive discretization strategy, uses the condition number of the Jacobian matrix and the preset threshold to determine the regional sensitivity, performs refined sampling of the upper limb workspace, and implements an oversampling strategy for boundary areas and singular areas. It then combines the kd-tree method to achieve efficient online nearest neighbor matching, and finally uses a local optimization method to solve the inverse kinematics problem of the human upper limb, ensuring the continuity, smoothness and real-time performance of the joint angles, providing solid and reliable technical support for the motion control of rehabilitation robots.

[0139] In the offline stage, the accessible workspace of the upper limb is discretely sampled based on the Monte Carlo sampling method. The sampling density is dynamically adjusted using the sensitivity index (the condition number of the Jacobian matrix), and finer sampling spacing is adopted in singular areas and boundary areas, thereby obtaining high-precision, comprehensive coverage of discrete point sampling. After obtaining each joint angle corresponding to each discrete point through the Jacobian matrix iteration method, the joint angles that do not meet the accuracy requirements are eliminated through the positive kinematic function, and the joint angle database is constructed using the final discrete point set and the corresponding set of joint angles after elimination.

[0140] In the online stage, an angle range query mechanism is introduced. Based on the doctor's targeted assessment results of the patient's upper limb joint motion range, the joint motion range of each joint in the current stage is obtained, and invalid joint angle combinations in the joint angle database are further screened. Personalized screening is achieved without rebuilding the joint angle database, and a subset of the joint angle database is obtained. The joint angle database subset is indexed using the efficient kd-tree method to quickly find the discrete point closest to the end position, and its corresponding joint angle is used as the initial joint angle. Based on the high-quality initial joint angle, it is further adjusted through local optimization algorithms such as the gradient descent method to ensure the continuity, smoothness and real-time performance of the joint angle.

[0141] In an exemplary embodiment, taking an upper limb rehabilitation training trajectory collected by a rehabilitation center as an example, the entire motion state determination process based on inverse kinematics of the human upper limb is described in detail.

[0142] 1. Obtain the upper limb working space of the human upper limb.

[0143] According to the structure of the human upper limb, a seven-degree-of-freedom kinematic model of the human upper limb is established to determine the joint angle vector. The expression of the joint angle vector is:

[0144] .

[0145] Among them, the angles of each joint meet .

[0146] For example, the joint angle ranges for the internal rotation / external rotation degrees of freedom of the shoulder joint, the flexion / extension degrees of freedom of the elbow joint, and the palmar flexion / dorsiflexion degrees of freedom of the wrist joint can be set to 、 and The remaining degrees of freedom of the upper limbs can be set according to actual physiological data.

[0147] The Denavit-Hartenberg method is used to construct the forward kinematic model:

[0148] .

[0149] Used to calculate the end pose at a given joint angle .

[0150] The upper limb working space is determined based on the forward kinematics model.

[0151] 2. Determine the initial discrete point set.

[0152] In the upper limb workspace, the Monte Carlo method is used to generate the initial discrete point set by preliminary uniform sampling. Let the total number of discrete points of the preliminary uniform sampling be = 5000, initial sampling interval These sampling points cover most of the upper limb workspace.

[0153] 3. Adaptive sampling density adjustment.

[0154] Determine the discrete points in the initial discrete point set by Jacobi matrix iteration method ( ) corresponding to the joint angle , and calculate its Jacobian matrix The condition number:

[0155] .

[0156] in, is the condition number of the Jacobian matrix, is the maximum singular value of the Jacobian matrix, is the minimum singular value of the Jacobian matrix.

[0157] When detected (i.e., sensitivity threshold ), it indicates that the kinematic changes in the area where the point is located are drastic. In the neighborhood of the discrete point, that is, the sensitive area, refined sampling is used to reduce the sampling interval to:

[0158] .

[0159] in, is the initial sampling spacing when using the Monte Carlo sampling method for preliminary uniform sampling, It is the sensitive sampling interval when performing refined sampling on sensitive areas.

[0160] For the boundary area (i.e., the sampling points less than 5% away from the workspace boundary) and the singular area, an oversampling strategy is further adopted, and the sampling interval is set to:

[0161] 。

[0162] 。

[0163] in, is the boundary sampling spacing when refining the boundary area. is the singular sampling spacing when refining the sampling of the singular region, where .

[0164] The final discrete point set is obtained by adaptive sampling, and the total number of discrete points in the final discrete point set is approximately point.

[0165] 4. Build a joint angle database.

[0166] The joint angle corresponding to each discrete point in the final discrete point set is calculated using the Jacobian matrix iteration method to obtain the joint angle set of each discrete point. Usually, each discrete point corresponds to several joint angles.

[0167] Through the positive kinematic function Determine the end position corresponding to each joint angle , the calculation formula is as follows:

[0168] .

[0169] The end position Target end position Compare and calculate the position error , the calculation formula is as follows:

[0170] .

[0171] Setting the position error threshold , determine whether the position error is greater than the preset position error threshold. If so, it means that the joint angle does not meet the accuracy requirements and the joint angle is eliminated. The judgment formula is as follows:

[0172] .

[0173] After removing the joint angles in the joint angle set, a removed joint angle set is obtained; and a joint angle database is constructed based on the final discrete point set and the corresponding removed joint angle set.

[0174] 5. Determine the initial joint angle.

[0175] In the experiment, the collected upper limb rehabilitation training trajectory contains 300 frames of end-point postures, and the end-point posture of each frame includes end-point position and posture information.

[0176] During the rehabilitation training, the upper limb end posture sequence is collected through a high-precision motion capture system. The end posture sequence expression is as follows:

[0177] .

[0178] in, For the k The end pose of the frame, including the end position and posture information.

[0179] The upper limb rehabilitation training trajectory is preprocessed. During the preprocessing process, the upper limb rehabilitation training trajectory is filtered to ensure that the position error of each frame data does not exceed 1mm and the posture error is controlled within 2°.

[0180] Introducing an angle range query mechanism, which can obtain the current range of motion of each joint based on the doctor's targeted assessment of the patient's upper limb joint range of motion ,in, is the minimum joint angle of each joint at the current stage, is the maximum joint angle of each joint at the current stage, and the expressions of the two are as follows:

[0181] ;

[0182] .

[0183] This angle range reflects the range of motion of each joint that the patient can currently achieve physiologically and safely, and is evaluated by the doctor and input into the system as a constraint condition.

[0184] The system filters the joint angle database according to the following logic:

[0185] .

[0186] in, Used to determine a candidate joint angle vector in the joint angle database q Whether it is within the patient's personalized angle range. That is, when all joint angles are within the patient's currently allowed range of motion, the angle solution is judged to be True and retained for subsequent construction of the joint angle database subset; otherwise, it is eliminated to avoid generating motion paths that are not suitable for the patient.

[0187] The final retained joint angle database subset The expression is:

[0188] .

[0189] in, It is the joint angle database.

[0190] In this way, according to the personalized rehabilitation needs of each patient, invalid joint angle combinations in the joint angle database are dynamically filtered, and personalized screening is achieved without rebuilding the joint angle database, so as to obtain the final joint angle database subset to be retrieved.

[0191] According to the upper limb rehabilitation training trajectory, the final discrete point set in the joint angle database subset is indexed based on the kd-tree method to support online efficient retrieval. The final discrete point set in this application includes 20,000 discrete points.

[0192] Extract the end position from the 300 frames of end pose collected in real time , for example, the target position of a certain frame is mm, use the kd-tree method to search the joint angle database subset and calculate the Euclidean distance between each discrete point and the current end position. The calculation formula is as follows:

[0193] .

[0194] Select the discrete point with the smallest Euclidean distance Corresponding joint angle As the initial joint angle. For example, mm, retrieve the initial joint angle , making the error less than 2mm.

[0195] 6. Based on initial joint angle Perform local optimization to determine the joint angles corresponding to each end pose. To match the initial joint angles obtained As the starting point, construct the objective function:

[0196] .

[0197] Among them, the position error weight , angle continuity error weight and joint angular velocity error weights They were set to 0.3, 0.5 and 0.2 respectively, and a good balance was achieved through experimental debugging.

[0198] The angle normalization function is defined as:

[0199] .

[0200] The gradient descent method is used to locally optimize the initial joint angle, and the update formula is:

[0201] .

[0202] Among them, the learning rate Set to 0.01, () is used to determine the gradients of each component of the joint angle using the finite difference method.

[0203] Among them, the joint angle The gradient of the components The calculation formula is:

[0204] .

[0205] in, Represents the objective function, that is, the joint angle vector Each component of imposes different error penalties, such as end position error, angle continuity error, joint change rate, etc. is the joint angle vector Component, that is, The angle of the joint. Indicates that Middle Add a small amount The objective function value is calculated later. It represents the small positive perturbation used in finite difference calculations, usually set to 0.001.

[0206] In this embodiment, local optimization generally reaches the error threshold within 3-5 iterations. mm. The end position is determined by the forward kinematics model based on the optimized joint angle. For the k The end position of the frame.

[0207] The above processing is performed on all 300 frames of end poses, and a set of continuous joint angle sequences are output. , taking the time to solve each frame and the overall end position error as evaluation indicators, it is verified whether the joint angle changes smoothly and meets the real-time motion control requirements.

[0208] 7. Determine the motion state of the human upper limbs based on each end position and the corresponding joint angle.

[0209] The joint angle database construction and corresponding joint angle determination process in this embodiment can be integrated into the rehabilitation robot control system. The system first constructs a high-quality joint angle database offline. Then, during actual rehabilitation training, the system collects the patient's upper limb rehabilitation training trajectory in real time. By searching and optimizing the joint angle database, the system calculates the joint angle sequence in real time. Feedback control is then used to achieve precise assisted upper limb movement.

[0210] In an exemplary embodiment, a motion state determination system based on inverse kinematics of a human upper limb is provided. The motion state determination system based on inverse kinematics of a human upper limb includes:

[0211] An upper limb workspace acquisition module is used to obtain the upper limb workspace of the human upper limb;

[0212] An initial discrete point set determination module is used to perform discrete sampling based on the upper limb workspace and the Monte Carlo sampling method to determine an initial discrete point set;

[0213] A final discrete point set determination module is configured to adjust the sampling density of the sensitive area, boundary area, and singular area in the initial discrete point set to obtain a final discrete point set; the sensitive area is an area where the sensitivity index corresponding to the discrete point is greater than a preset sensitivity threshold; the boundary area is an area where the distance between the discrete point and the boundary of the upper limb workspace is less than a preset distance threshold; the singular area is an area where the singularity index corresponding to the discrete point is greater than a preset singularity threshold; the sensitivity index is the Jacobian matrix condition number of the joint angle corresponding to the discrete point; and the singularity index is the same as the sensitivity index;

[0214] a joint angle database determination module, configured to determine a joint angle set corresponding to each discrete point in the final discrete point set; and construct a joint angle database based on the final discrete point set and the corresponding joint angle set;

[0215] An initial joint angle determination module is configured to determine the initial joint angle by searching the joint angle database based on the kd-tree method according to the upper limb rehabilitation training trajectory; the upper limb rehabilitation training trajectory includes the end position at different times; the end position includes: end position and posture information;

[0216] a joint angle determination module, configured to locally optimize the initial joint angle based on a gradient descent method to determine the joint angle corresponding to each end position;

[0217] The motion state determination module is used to determine the motion state of the human upper limb based on each end position and the corresponding joint angle.

[0218] In an exemplary embodiment, a computer device is provided, which may be a server or a terminal. The computer device includes a processor, memory, an input / output (I / O) interface, and a communication interface. The processor, memory, and I / O interface are connected via a system bus, and the communication interface is connected to the system bus via the I / O interface. The processor of the computer device provides computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores an operating system, a computer program, and a database. The internal memory provides an environment for the operating system and computer program in the non-volatile storage medium to run. The database of the computer device stores data for determining the motion state of a human upper limb based on inverse kinematics. The I / O interface of the computer device is used to exchange information between the processor and an external device. The communication interface of the computer device is used to communicate with an external terminal via a network connection. When executed by the processor, the computer program implements a method for determining the motion state of a human upper limb based on inverse kinematics.

[0219] In an exemplary embodiment, a computer device is provided, including a memory and a processor. The memory stores a computer program, and the processor implements the steps in the above method embodiments when executing the computer program.

[0220] In an exemplary embodiment, a computer-readable storage medium is provided, storing a computer program. When the computer program is executed by a processor, the steps in the above-mentioned method embodiments are implemented.

[0221] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of relevant data must comply with relevant regulations.

[0222] Those skilled in the art will appreciate that all or part of the processes in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the above-mentioned embodiments. In particular, any reference to memory, database, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM may be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0223] The databases involved in the various embodiments provided herein may include at least one of a relational database and a non-relational database. Non-relational databases may include, but are not limited to, distributed databases based on blockchains. The processors involved in the various embodiments provided herein may include, but are not limited to, general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic units, data processing logic units based on quantum computing, and the like.

[0224] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above 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.

[0225] This document uses specific examples to illustrate the principles and implementation methods of this application. The description of the above examples is only intended to help understand the method and core concept of this application. At the same time, for those skilled in the art, based on the concept of this application, there may be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A method for determining the motion state of a human upper limb based on inverse kinematics, characterized in that: The motion state determination method based on inverse kinematics of human upper limbs includes: Obtaining the upper limb working space of the human upper limb; According to the upper limb workspace, discrete sampling is performed based on the Monte Carlo sampling method to determine an initial discrete point set; Adjusting the sampling density of the sensitive area, boundary area and singular area in the initial discrete point set to obtain a final discrete point set; Determining a joint angle set corresponding to each discrete point in the final discrete point set; and constructing a joint angle database based on the final discrete point set and the corresponding joint angle set; According to the upper limb rehabilitation training trajectory, the joint angle database is searched based on the kd-tree method to determine the initial joint angle; the upper limb rehabilitation training trajectory includes the end position at different times; the end position includes: end position and posture information; Performing local optimization on the initial joint angle based on the gradient descent method to determine the joint angle corresponding to each end position; Determine the motion state of the human upper limb based on each end position and the corresponding joint angle; The process of determining the sensitive area includes: Using the formula and formula determining the sensitive area; in, is the joint angle corresponding to the discrete point in the initial discrete point set, is the Jacobian matrix of joint angle to end pose, is the condition number of the Jacobian matrix, is the preset sensitivity threshold, is the maximum singular value of the Jacobian matrix, is the minimum singular value of the Jacobian matrix.

2. The method for determining the motion state of a human upper limb based on inverse kinematics according to claim 1, wherein: The process of determining the upper limb workspace includes: Based on the human upper limb kinematic model, determine the joint angle vector; According to the joint angle vector, a forward kinematics model is constructed based on the Denavit-Hartenberg parameter method; The upper limb working space is determined based on the forward kinematics model.

3. The method for determining the motion state of a human upper limb based on inverse kinematics according to claim 1, wherein: The step of constructing a joint angle database based on the final discrete point set and the corresponding joint angle set specifically includes: Eliminate the joint angles in the joint angle set based on the positive kinematics function to obtain a set of eliminated joint angles; A joint angle database is constructed based on the final discrete point set and the corresponding joint angle set after elimination.

4. The method for determining the motion state of a human upper limb based on inverse kinematics according to claim 1, wherein: The method of searching the joint angle database based on the upper limb rehabilitation training trajectory and determining the initial joint angle based on the kd-tree method specifically includes: Filtering the joint angle database based on an angle range query mechanism to obtain a joint angle database subset; According to the upper limb rehabilitation training trajectory, a subset of the joint angle database is searched based on the kd-tree method; According to the search results, determine the Euclidean distance between each discrete point and the current end position; The joint angle corresponding to the discrete point with the smallest Euclidean distance is taken as the initial joint angle.

5. The method for determining the motion state of a human upper limb based on inverse kinematics according to claim 4, wherein: The process of determining the Euclidean distance includes: Using the formula Determine the Euclidean distance between each discrete point and the current end position; in, For the Frame end position The joint angle database subset j discrete points The Euclidean distance of For the The frame end position is x The coordinates on the axis, For discrete points exist x The coordinates on the axis, For the The frame end position is y The coordinates on the axis, For discrete points exist y The coordinates on the axis, For the The frame end position is z The coordinates on the axis, For discrete points exist z Coordinates on the axis.

6. The method for determining the motion state of a human upper limb based on inverse kinematics according to claim 2, wherein: The local optimization of the initial joint angle based on the gradient descent method to determine the joint angle corresponding to each end position specifically includes: Using the formula Determine the objective function ;in, For joint angle vectors Each component of imposes a different error penalty, is the position error weight, is the end position determined by the forward kinematics model, is the target end position, is the angle continuity error weight, is the frame number, is the total number of joints, For the k The joint angles of each frame, For the -1 frame corresponding to each joint angle, is the angle normalization function, is the joint angular velocity error weight, is the time rate of change of the joint angle; Using the formula Perform local optimization on the initial joint angles; For the The joint angle corresponding to the end pose of the frame, is the initial joint angle, is the learning rate, () is used to determine the gradients of each component of the joint angle using the finite difference method.

7. A motion state determination system based on inverse kinematics of a human upper limb, for implementing the motion state determination method based on inverse kinematics of a human upper limb according to any one of claims 1 to 6, characterized in that: The motion state determination system based on inverse kinematics of human upper limbs includes: An upper limb workspace acquisition module is used to obtain the upper limb workspace of the human upper limb; An initial discrete point set determination module is used to perform discrete sampling based on the upper limb workspace and the Monte Carlo sampling method to determine an initial discrete point set; A final discrete point set determination module is used to adjust the sampling density of the sensitive area, boundary area and singular area in the initial discrete point set to obtain a final discrete point set; a joint angle database determination module, configured to determine a joint angle set corresponding to each discrete point in the final discrete point set; and construct a joint angle database based on the final discrete point set and the corresponding joint angle set; An initial joint angle determination module is configured to determine the initial joint angle by searching the joint angle database based on the kd-tree method according to the upper limb rehabilitation training trajectory; the upper limb rehabilitation training trajectory includes the end position at different times; the end position includes: end position and posture information; a joint angle determination module, configured to locally optimize the initial joint angle based on a gradient descent method to determine the joint angle corresponding to each end position; The motion state determination module is used to determine the motion state of the human upper limb based on each end position and the corresponding joint angle.

8. A computer device comprising: A memory, a processor, and a computer program stored in the memory and runnable on the processor, characterized in that the processor executes the computer program to implement the motion state determination method based on inverse kinematics of the human upper limb according to any one of claims 1-6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method for determining the motion state based on inverse kinematics of a human upper limb according to any one of claims 1 to 6 is implemented.

Images