A human-machine motion mapping control system and method based on hybrid LM-GA algorithm

Abstract

The present invention discloses a human-machine motion mapping control system and method based on a hybrid LM-GA algorithm, which relates to the field of human-machine motion mapping control. The present invention obtains the arm joint position information from an optical motion capture system and uses a geometric solution in inverse kinematics to obtain the corresponding arm joint angle. Then, an inverse kinematics iterative algorithm is used to obtain the joint angle data of a robotic arm and verify it in a virtual environment. Then, the optical motion capture system is used to obtain the trajectory data of the end of the robotic arm. By comparing the trajectory data of the human arm end and the robotic arm end, the reproduction accuracy of the robotic arm motion can be evaluated, thereby ensuring the accuracy and reliability of the human-machine motion mapping system. In addition, an improved iterative algorithm is adopted. By continuously approaching the optimal solution, the iterative algorithm can gradually adjust the angle of the robotic arm joint until a predetermined end effector position is reached.

Description

Technical Field

[0001] The present invention belongs to the field of human-machine motion mapping control, and in particular, relates to a human-machine motion mapping control system and method based on a hybrid LM-GA algorithm. Background Art

[0002] Human-machine motion mapping is an advanced interactive technology that captures and analyzes human movements, converting them into machine commands or control signals, enabling seamless communication between humans and machines. This technology is widely used in fields such as virtual reality, robotic control, game interaction, and sports rehabilitation.

[0003] In recent years, human-machine motion mapping technology has attracted much attention in the field of precision assembly, especially in the 3C (computer, communication, and consumer electronics) assembly process, which requires high precision and efficiency. Researchers have applied human-machine motion mapping technology to the field of precision assembly and have achieved practical and effective results in the research of precision assembly systems.

[0004] The aforementioned high-precision assembly systems demonstrate exceptional performance and precision in the micro-assembly field. However, the system's design complexity can lead to high costs and maintenance requirements, requiring regular calibration to ensure continued high-precision performance. Furthermore, the system's adaptation to specific parts may require specialized grippers and adjustments, which to some extent limits its versatility. With the increasing standardization of assembly technology and processes, the application of robots in the 3C industry has grown rapidly, and there is a strong demand for automated transformation. Although modern robotic arms have high repeatability, they are still not accurate enough for certain assembly tasks in the 3C industry that require high precision. Furthermore, current vision systems and sensor technologies are still limited in their ability to handle complex scenes and tiny objects, and are also subject to issues such as latency and trajectory confusion. Summary of the Invention

[0005] In response to the problems in the related art, the present invention proposes a human-machine motion mapping control system and method based on a hybrid LM-GA algorithm to overcome the above technical problems existing in the existing related art.

[0006] To solve the above technical problems, the present invention is achieved through the following technical solutions:

[0007] The present invention is a human-machine motion mapping control method based on a hybrid LM-GA algorithm, comprising the following steps:

[0008] S1. Adjust the positions and angles of multiple cameras of the optical motion capture system; after the adjustment is completed, use the optical motion capture system to collect the arm joint position information data of the experimenter to obtain the original arm joint position information;

[0009] S2. Preprocessing the original arm joint position information to obtain processed arm joint position information;

[0010] S3. Using a geometric solution in inverse kinematics to obtain arm joint angle data corresponding to the processed arm joint position information;

[0011] S4. Calculating the arm joint angle data using an inverse kinematics iterative algorithm to obtain the joint angle data of the robotic arm;

[0012] S5. Verifying the joint angle data of the robotic arm in a virtual environment; during the verification process, using an optical motion capture system to obtain trajectory data of the end of the robotic arm; comparing the trajectory data of the end of the robotic arm with the trajectory data of the end of the human arm to obtain a reproduction accuracy evaluation result of the robotic arm movement;

[0013] In order to ensure that the optical motion capture device can provide accurate and reliable three-dimensional spatial data, before collecting data from the optical motion capture system, it is first necessary to adjust the position and angle of each camera to ensure that they can all cover a common capture area, and then calibrate the system; when collecting human motion data, the experimenter is required to stick reflective markers on the joints of the arm, and obtain the coordinate data of all reflective points through the optical motion capture system; the raw data obtained by the system may be affected by many factors, such as the operator's unconscious shaking, occlusion of markers, and even changes in ambient light; these problems may lead to data errors, noise, and even incompleteness; therefore , it is particularly important to effectively preprocess these raw data. At the same time, different preprocessing effects will affect the accuracy of trajectory reproduction. Another function of data preprocessing is to calculate the joint angles of each joint of the arm. First, based on the arm joint position information obtained by the optical motion capture system, the corresponding arm joint angles are obtained using the geometric solution in inverse kinematics. Secondly, the inverse kinematics iterative algorithm is used to obtain the joint angle data of the robotic arm and verify it in a virtual environment first. Then, the optical motion capture system is used to obtain the trajectory data of the end of the robotic arm. By comparing the trajectory data of the end of the human arm and the end of the robotic arm, the reproduction accuracy of the robotic arm motion can be evaluated to ensure the accuracy and reliability of the human-machine motion mapping system.

[0014] Preferably, the original arm joint position information in S1 includes displacement, velocity and acceleration data in the world coordinate system.

[0015] Preferably, said S2 comprises the following steps:

[0016] S21, performing data filling using cubic interpolation according to the positional relationship between the visible points and the missing points in the original arm joint position information to obtain filled data;

[0017] S22, using a Savitzky-Golay quinary cubic filtering method to remove noise in the motion trajectory of the padded data to obtain processed arm joint position information;

[0018] To further improve the accuracy of motion reproduction and eliminate possible noise, errors, and incompleteness in the original data, the positional relationship between visible points and missing points is first used to fill in the data using cubic interpolation. To solve cubic spline interpolation, it is usually necessary to construct a system of linear equations whose coefficients are determined by the values ​​of the data points and the endpoint conditions. Then, the Gaussian elimination method can be used to solve this system of equations to obtain the coefficients of each piecewise cubic polynomial, providing a smooth curve while maintaining the interpolation accuracy of the data points. To eliminate noise in the original data caused by jitter during the operator's hand movement, the Savitzky-Golay five-point cubic filtering method is used to remove noise from the motion trajectory. In the time domain, based on a second-order polynomial, the least squares method is used to perform optimal fitting through a moving window, meeting the purpose of SG filtering to smooth the trajectory without deviating from the original data.

[0019] Preferably, the S4 comprises the following steps:

[0020] S41. Using the improved DH parameter method, the geometric relationship between the joints of the robotic arm and the position and posture of the end effector are modeled to obtain a robotic arm model and an end effector model;

[0021] Preferably, the S41 includes the following steps:

[0022] S411. Set the robotic arm joint set a = {a1, a2, a3, a4, a5, a6}, where a1, a2, a3, a4, a5, and a6 represent the first joint, second joint, third joint, fourth joint, fifth joint, and sixth joint of the robotic arm, respectively; define parameters for each robotic arm joint in the robotic arm joint set to obtain a robotic arm joint parameter matrix a′; as follows,

[0023]

[0024] Among them, a i ′1, a i ′2, a i ′3, a i ′4, a i ′5 represents the joint angle parameter of the i-th manipulator joint in the manipulator joint set, the translation distance parameter along the previous joint axis, the translation distance parameter along the current joint axis, the angle parameter between the current joint axis and the previous joint axis, and the joint angle range parameter;

[0025] S412, calculating the transformation matrix of the transformation from the i-1th manipulator joint coordinate system to the i-th manipulator joint coordinate system in the manipulator joint set according to the manipulator joint parameter matrix a′ as follows,

[0026]

[0027] The pose transformation matrix of the end effector relative to the base coordinate system The calculation formula is as follows,

[0028]

[0029] Preferably, the S5 comprises the following steps:

[0030] S51, performing a human-machine motion model conversion operation based on the robot arm model and the end effector model and using an iterative numerical solution of a hybrid LM-GA;

[0031] S52, using an optical motion capture system to obtain trajectory data of the end of the robotic arm during the human-machine motion model conversion operation; comparing the trajectory data of the end of the robotic arm with the trajectory data of the end of the human arm to obtain a reproduction accuracy evaluation result of the robotic arm motion;

[0032] An optical motion capture device is used to capture the coordinate position of the end of the human arm. These coordinate data not only provide the precise end-effector trajectory for the robot arm, but also, through spatial geometric analysis, can further calculate the real-time angle value of the arm's wrist joint. The wrist joint angle value is used as a constraint condition for the robot arm's joint motion to ensure that the robot arm's motion conforms to the anthropomorphic trajectory while maintaining efficiency and precision. In order to achieve the goal of human-machine mapping, an improved iterative algorithm is used to complete the inverse kinematics analysis of the robot arm. Inverse kinematics is the process of determining the angles of each joint of the robot arm to achieve the desired end position and posture, and the iterative algorithm provides an effective solution in this process. By continuously approaching the optimal solution, the iterative algorithm can gradually adjust the angles of the robot arm joints until the predetermined end-effector position is reached.

[0033] Preferably, the S51 includes the following steps:

[0034] S511, encoding the joint angle to obtain the joint angle code; constructing a chromosome population and initializing the chromosome population according to the joint angle code;

[0035] S512, using a fitness function to calculate the fitness of each chromosome in the chromosome population to obtain a fitness value set; selecting individuals from the chromosome population according to the fitness value set to obtain selected individuals; when the selected individuals meet the requirements, using the selected individuals as initial values ​​of the LM algorithm;

[0036] Otherwise, individual crossover and individual mutation operations are performed on the chromosome population, and S512 is repeated until the selected individuals meet the requirements; the fitness function is as follows:

[0037]

[0038] Where: θ represents the joint angle; fitness(θ) represents the fitness function of θ; d(θ) represents the Euclidean distance between the position of the end effector at the joint angle θ and the target position; σ represents a constant used to control the influence range of the distance term; JointLimit(θ) represents a penalty function that takes a positive value when the joint angle exceeds the limit and zero otherwise; ω1 and ω2 are weight coefficients used to balance the importance of distance accuracy and joint limits;

[0039] S513, constructing an objective function; initializing the parameters of the LM algorithm and calculating the error of the objective function; when the error of the objective function meets the requirement, the algorithm ends; otherwise, re-initializing the parameters of the LM algorithm and repeating S513 until the error of the objective function meets the requirement;

[0040] Preferably, the S513 includes the following steps:

[0041] S5131, setting the pose matrix of the end effector of the human arm; substituting the current joint angle of the robotic arm into the pose transformation matrix of the end effector relative to the base coordinate system to obtain the current pose matrix of the end effector of the robotic arm;

[0042] S5132, calculating the position error of the end effector of the robotic arm according to the pose matrix of the end effector of the human arm and the current pose matrix of the end effector of the robotic arm;

[0043] The algorithm achieves convergence by dynamically adjusting the iterative step size by introducing the adjustment of the damping factor parameters. During the iterative solution process, the LM algorithm balances the calculation accuracy and error reduction by adjusting the parameters, so that the geometric parameter error continuously approaches the accurate value; when the parameter setting is large, the LM algorithm is similar to the gradient descent method, which helps the algorithm to perform a global search in the entire solution space, thereby ensuring that the algorithm can find the global optimal solution or at least a stable solution in the global range; when the parameter setting is small, the LM algorithm is closer to the Gauss-Newton method. At this time, the convergence speed of the algorithm in the local area will be accelerated, which helps to quickly approach the exact location of the solution.

[0044] A human-machine motion mapping control system based on a hybrid LM-GA algorithm includes an arm joint position information data acquisition module, a preprocessing module, an arm joint angle data acquisition module, a robotic arm joint angle data calculation module, a robotic arm end trajectory data acquisition module, and a robotic arm motion accuracy evaluation module;

[0045] The arm joint position information data acquisition module is used to adjust the positions and angles of multiple cameras of the optical motion capture system; after the adjustment is completed, the optical motion capture system is used to collect the arm joint position information data of the experimenter during movement to obtain the original arm joint position information;

[0046] The preprocessing module is used to preprocess the original arm joint position information to obtain processed arm joint position information;

[0047] The arm joint rotation angle data acquisition module is used to obtain the arm joint rotation angle data corresponding to the processed arm joint position information by using a geometric solution in inverse kinematics;

[0048] The robotic arm joint angle data calculation module is used to calculate the arm joint angle data using an inverse kinematics iterative algorithm to obtain the robotic arm joint angle data;

[0049] The robot arm end trajectory data acquisition module is used to verify the joint angle data of the robot arm in a virtual environment; during the verification process, an optical motion capture system is used to obtain the trajectory data of the robot arm end;

[0050] The robot arm motion accuracy assessment module is used to compare the trajectory data of the end of the robot arm with the trajectory data of the end of the human arm to obtain a reproducible accuracy assessment result of the robot arm motion.

[0051] The present invention has the following beneficial effects:

[0052] 1. In the present invention, the arm joint position information obtained by the optical motion capture system is used to obtain the corresponding arm joint angle using the geometric solution in inverse kinematics. Secondly, the inverse kinematics iterative algorithm is used to obtain the joint angle data of the robotic arm and first verify it in a virtual environment; then the optical motion capture system is used to obtain the trajectory data of the end of the robotic arm. By comparing the trajectory data of the end of the human arm and the end of the robotic arm, the reproduction accuracy of the robotic arm movement can be evaluated to ensure the accuracy and reliability of the human-machine motion mapping system.

[0053] 2. The present invention utilizes the positional relationship between visible and missing points and employs cubic interpolation to fill in the data. This method obtains the coefficients of each piecewise cubic polynomial, maintaining the interpolation accuracy of the data points while providing a smooth curve. A Savitzky-Golay five-point cubic filter is used to remove noise from the motion trajectory. In the time domain, a moving window least squares method is used for optimal fitting based on a second-order polynomial. This achieves the goal of SG filtering to smooth the trajectory without deviating from the original data.

[0054] 3. The present invention adopts an improved iterative algorithm to complete the inverse kinematics analysis of the robotic arm. Inverse kinematics is the process of determining the angles of each joint of the robotic arm to achieve the desired end position and posture, and the iterative algorithm provides an effective solution in this process; by continuously approaching the optimal solution, the iterative algorithm can gradually adjust the angles of the robotic arm joints until the predetermined end effector position is reached.

[0055] Of course, any product implementing the present invention does not necessarily need to achieve all of the advantages described above at the same time. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] In order to more clearly illustrate the technical solutions of the embodiments of the invention, the following briefly introduces the drawings required for describing the embodiments. Obviously, the drawings described below are only some embodiments of the invention. For ordinary technicians in this field, they can also obtain drawings based on these drawings without paying any creative work.

[0057] Figure 1 Schematic diagram of a flow chart of a human-machine motion mapping control method based on a hybrid LM-GA algorithm of the present invention;

[0058] Figure 2 Schematic diagram of the kinematic model of the robotic arm of the present invention;

[0059] Figure 3 This is a schematic diagram of a simplified model of the shoulder joint of the present invention;

[0060] Figure 4 This is a diagram showing the joints of the arm of the present invention and the robotic arm;

[0061] Figure 5 Schematic diagram of the basic process of the LM-GA algorithm of the present invention, wherein GA represents genetic algorithm and LM represents Levenberg-Marquardt method;

[0062] Figure 6 This is a framework diagram of the experimental system of the present invention;

[0063] Figure 7 It is a dot map of the arm of the present invention;

[0064] Figure 8 This is the arm experimental action posture diagram of the present invention;

[0065] Figure 9 This is a graph showing data collected by the optical motion capture system of the present invention;

[0066] Figure 10 Schematic diagram of position coordinates in the time series data sample of the experimental action of the present invention, where Px represents the coordinate position on the X-axis, Py represents the coordinate position on the Y-axis, and Pz represents the coordinate position on the Z-axis;

[0067] Figure 11 Schematic diagram of velocity coordinates in the time series data sample of the experimental action of the present invention, where Vx represents the velocity component on the X-axis, Vy represents the velocity component on the Y-axis, Vz represents the velocity component on the Z-axis, and V represents the combined velocity of the three axes;

[0068] Figure 12 This is a schematic diagram of the acceleration coordinates in the time series data sample of the experimental action of the present invention, where a x Represents the acceleration component on the X-axis, a Y Represents the acceleration component on the Y axis, a z represents the acceleration component on the Z axis, and a represents the three-axis combined acceleration;

[0069] Figure 13 This is a schematic diagram of data before cubic interpolation of the present invention;

[0070] Figure 14 This is a schematic diagram of data after cubic interpolation of the present invention;

[0071] Figure 15 This is a comparison chart of the SG filtering effect of the present invention. The upper figure is a schematic diagram of the original data, and the lower figure is a schematic diagram of the data after SG filtering;

[0072] Figure 16 Schematic diagram of SG filtering error of the present invention;

[0073] Figure 17 The trajectory of the end effector of the arm in three-dimensional space of the present invention;

[0074] Figure 18 The trajectory of the end effector of the robot arm in three-dimensional space of the present invention;

[0075] Figure 19 Schematic diagram of the projection of the end trajectory of the present invention on the xy plane. The upper figure is the projection of the arm trajectory on the xy plane, and the lower figure is the projection of the robot arm trajectory on the xy plane;

[0076] Figure 20Schematic diagram of the projection of the end trajectory of the present invention on the xz plane. The upper figure is the projection of the arm trajectory on the xz plane, and the lower figure is the projection of the robot arm trajectory on the xz plane;

[0077] Figure 21 This is a schematic diagram of the projection of the end trajectory of the present invention on the yz plane. The upper figure is the projection of the arm trajectory on the yz plane, and the lower figure is the projection of the robot arm trajectory on the yz plane. DETAILED DESCRIPTION

[0078] The following will clearly and completely describe the technical solutions in the embodiments of the invention in conjunction with the accompanying drawings. Obviously, the embodiments described are only part of the embodiments of the invention, not all of them. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0079] In the description of the present invention, it should be understood that the terms "opening", "upper", "lower", "top", "middle", "inside" and the like indicating orientation or positional relationship are only for the convenience of describing the invention and simplifying the description, and do not indicate or imply that the components or elements referred to must have a specific orientation, be constructed and operate in a specific orientation, and therefore cannot be understood as limiting the invention.

[0080] Example 1

[0081] Reference Figure 1 As shown, this embodiment is a human-machine motion mapping control method based on a hybrid LM-GA algorithm, comprising the following steps:

[0082] S1. Adjust the positions and angles of multiple cameras of the optical motion capture system; after the adjustment is completed, use the optical motion capture system to collect the arm joint position information data of the experimenter to obtain the original arm joint position information;

[0083] S2. Preprocessing the original arm joint position information to obtain processed arm joint position information;

[0084] The S2 comprises the following steps:

[0085] S21, performing data filling using cubic interpolation according to the positional relationship between the visible points and the missing points in the original arm joint position information to obtain filled data;

[0086] S22, using a Savitzky-Golay quinary cubic filtering method to remove noise in the motion trajectory of the padded data to obtain processed arm joint position information;

[0087] S3. Using a geometric solution in inverse kinematics to obtain arm joint angle data corresponding to the processed arm joint position information;

[0088] S4. Calculating the arm joint angle data using an inverse kinematics iterative algorithm to obtain the joint angle data of the robotic arm;

[0089] Reference Figure 2 As shown, the S4 includes the following steps:

[0090] S41. Using the improved DH parameter method, the geometric relationship between the joints of the robotic arm and the position and posture of the end effector are modeled to obtain a robotic arm model and an end effector model;

[0091] Due to the differences in kinematics and dynamics between the human arm and a robotic arm, an in-depth analysis of their joint structures is required. Based on robotics theory, the human arm can be equivalent to a seven-degree-of-freedom robotic arm with an SRS configuration. This is due to the two synovial joints at the wrist and the forearm's ability to pronate and supinate, which together form a ball-joint-like structure. This structure allows the human arm to exhibit a high degree of flexibility and dynamic adjustment capabilities when performing tasks.

[0092] The human arm joints are driven by bones and muscles. Specifically, when the human shoulder joint moves, although it appears to be a single movement overall, it actually involves the coordinated action of multiple rotation axes. A right-handed Cartesian coordinate system is established at the center of the shoulder joint, with the center of the elbow joint E as the reference point. When the center of the elbow joint moves from the original position E to the E' position, the human body will perform two rotational movements: one around the axis to change the position, and the other around the axis to adjust the posture. This dynamic adjustability of the rotation axis makes it possible for the human arm to perform complex movements in space. However, the joint axis of the robotic arm is related to the mechanical structure and cannot achieve the same flexible changes as the human arm. There are significant differences between the human arm and the robotic arm during movement, which brings difficulties to the mapping between the human arm and the robotic arm.

[0093] The S41 includes the following steps:

[0094] S411. Set the robotic arm joint set a = {a1, a2, a3, a4, a5, a6}, where a1, a2, a3, a4, a5, and a6 represent the first joint, second joint, third joint, fourth joint, fifth joint, and sixth joint of the robotic arm, respectively; define parameters for each robotic arm joint in the robotic arm joint set to obtain a robotic arm joint parameter matrix a′; as follows,

[0095]

[0096] Among them, ai ′1, a i ′2, a i ′3, a i ′4, a i ′5 represents the joint angle parameter of the i-th manipulator joint in the manipulator joint set, the translation distance parameter along the previous joint axis, the translation distance parameter along the current joint axis, the angle parameter between the current joint axis and the previous joint axis, and the joint angle range parameter; the specific parameter values ​​are shown in the following table,

[0097]

[0098] S412, calculating the transformation matrix of the transformation from the i-1th manipulator joint coordinate system to the i-th manipulator joint coordinate system in the manipulator joint set according to the manipulator joint parameter matrix a′ as follows,

[0099]

[0100] The pose transformation matrix of the end effector relative to the base coordinate system The calculation formula is as follows,

[0101]

[0102] S5. Verifying the joint angle data of the robotic arm in a virtual environment; during the verification process, using an optical motion capture system to obtain trajectory data of the end of the robotic arm; comparing the trajectory data of the end of the robotic arm with the trajectory data of the end of the human arm to obtain a reproduction accuracy evaluation result of the robotic arm movement;

[0103] The S5 comprises the following steps:

[0104] S51, performing a human-machine motion model conversion operation based on the robot arm model and the end effector model and using an iterative numerical solution of a hybrid LM-GA;

[0105] The S51 includes the following steps:

[0106] S511, encoding the joint angle to obtain the joint angle code; constructing a chromosome population and initializing the chromosome population according to the joint angle code;

[0107] S512, using a fitness function to calculate the fitness of each chromosome in the chromosome population to obtain a fitness value set; selecting individuals from the chromosome population according to the fitness value set to obtain selected individuals; when the selected individuals meet the requirements, using the selected individuals as initial values ​​of the LM algorithm;

[0108] Otherwise, individual crossover and individual mutation operations are performed on the chromosome population, and S512 is repeated until the selected individuals meet the requirements; the fitness function is as follows:

[0109]

[0110] Where: θ represents the joint angle; fitness(θ) represents the fitness function of θ; d(θ) represents the Euclidean distance between the position of the end effector at the joint angle θ and the target position; σ represents a constant used to control the influence range of the distance term; JointLimit(θ) represents a penalty function that takes a positive value when the joint angle exceeds the limit and zero otherwise; ω1 and ω2 are weight coefficients used to balance the importance of distance accuracy and joint limits;

[0111] S513, constructing an objective function; initializing the parameters of the LM algorithm and calculating the error of the objective function; when the error of the objective function meets the requirement, the algorithm ends; otherwise, re-initializing the parameters of the LM algorithm and repeating S513 until the error of the objective function meets the requirement;

[0112] Reference Figure 5 As shown in the figure, the core idea of ​​the LM-GA algorithm is to determine the initial value of the iteration through the genetic algorithm, use the iterative optimization of the LM algorithm, dynamically adjust the step size and damping factor, combine the gradient descent and Gauss-Newton method, and gradually approximate the joint angle solution that makes the end effector reach the desired position. The core steps of the algorithm are as follows:

[0113] Determination of the initial value of the iterative algorithm; the LM-GA algorithm first uses the fitness function defined by the genetic algorithm to evaluate the population to identify excellent individuals with high fitness; these excellent individuals are selected as the initial value of the LM algorithm for further solution, and then the result is used as the initial value of the LM algorithm again. After multiple iterative calculations, the optimal solution that meets the conditions is obtained;

[0114] The objective function is solved iteratively. The iterative mapping algorithm approaches the inverse kinematics problem of the robot by treating the process of finding the inverse solution as an optimization problem. This method uses constraints that simulate human control strategies and focuses on reducing the error between the robot's end effector and the predetermined target. Specifically, the objective function is constructed according to the following steps:

[0115] Assume that the pose matrix of the end of the human arm is as follows,

[0116]

[0117] Where n h 、o h 、a hThey represent the normal, pointing, and approach vectors of the end of the arm, and the three vectors are perpendicular to each other; R 3×3,h The matrix represents the posture information of the end of the arm relative to the base coordinate system, p h Represents the position vector of the end of the arm relative to the base coordinate system;

[0118] Set the current joint angle of the robot arm to θ r Substitute the pose transformation matrix of the end effector relative to the base coordinate system The current pose matrix of the end effector of the robotic arm is obtained from the calculation formula as follows:

[0119]

[0120] Where n r 、o r 、a r They represent the normal, pointing, and approach vectors of the end of the robotic arm, and the three vectors are perpendicular to each other; R 3×3,r The matrix represents the posture information of the end of the robot arm relative to the base coordinate system, p r Represents the position vector of the end of the manipulator relative to the base coordinate system;

[0121] In order to make the end effector of the robotic arm move according to the desired trajectory of the end of the arm, it is necessary to make the error between the position vector in the current pose matrix and the position vector in the desired pose matrix as small as possible. The position error of the end effector of the robotic arm can be approximately expressed as:

[0122]

[0123] Where Δθ i represents the angle error of the i-th joint; P represents the position of the end of the robot arm; θ i represents the angle of the i-th joint;

[0124] The above formula is written in matrix form as follows:

[0125] ΔP=J θ Δδ;

[0126] Among them J θ It is a 3×6 matrix, called the angle error matrix, that is,

[0127]

[0128] Among them, p x 、p y 、p z Respectively represent the position of the end of the robotic arm on the x-axis, y-axis, and z-axis;

[0129] Δδ is a 6×1 vector composed of the angle errors of the six joints, that is,

[0130] Δδ=(Δθ1,…,Δθ6);

[0131] In normal circumstances, J θ It is a singular square matrix, and the generalized inverse matrix is ​​needed to solve Δδ; the matrix J θ The generalized inverse matrix of is The solution of the linear equations is:

[0132]

[0133] The algorithm achieves convergence by dynamically adjusting the iterative step size by introducing the adjustment of the damping factor parameter μ. During the iterative solution process, the LM algorithm balances the calculation accuracy and error reduction by adjusting the parameter μ value, so that the geometric parameter error continues to approach the accurate value. When the parameter μ is set to a large value, the LM algorithm is similar to the gradient descent method, which helps the algorithm to perform a global search in the entire solution space, thereby ensuring that the algorithm can find the global optimal solution or at least a stable solution in the global range. When the parameter μ is set to a small value, the LM algorithm is closer to the Gauss-Newton method. At this time, the convergence speed of the algorithm in the local area will be accelerated, which helps to quickly approach the exact location of the solution. Its general formula is:

[0134]

[0135] Where, μ is the damping coefficient, μ>0; I is the unit matrix;

[0136] Joint constraints solved iteratively; Considering that the objective function of the above iteration has infinite solutions due to the lack of end-position constraints, and the rotation angle transformation of the end effector from the current position to the target position must be uniform and continuous to avoid sudden changes in the movement rate of the robot joint and impact on the joint; In the task of continuous path planning of the robot, it is assumed that there are a series of key nodes, denoted as Pk (k = 1, 2...); By applying the inverse kinematics iterative method of the robot arm, the corresponding joint angle vector is calculated for each node Pk, denoted as θ k ; The purpose of this process is to ensure that the currently calculated joint angle vector θ k With the previous solution θ k-1 As close as possible to maintain the continuity and smoothness of the robot's movement, thereby achieving a smooth transition of the robot's actions;

[0137] min||Δθ||=||θ k+1 -θ k || k=1,2…;

[0138] At the same time, the angle constraints of each joint of the robotic arm should be set as:

[0139] θi,min ≤θ i ≤θ i,max i=1,2…6;

[0140] Where θ i,min and θ i,max Respectively represent the corresponding joint lower limit and joint upper limit of the i-th joint;

[0141] To achieve anthropomorphic movement of the robotic arm, the real-time angle changes of the human arm's wrist joint are transmitted to the corresponding joints of the robotic arm. Specifically, an optical motion capture device is used to construct a geometric structure called an "arm triangle" by setting three reflective marker points on the arm, located at the wrist joint, elbow joint, and hand. The coordinate positions of the reflective marker points can accurately reflect the angle changes of the wrist joint and provide the angle value to the robotic arm.

[0142]

[0143] Where L forearm Represents the distance from the center of the human wrist joint to the hand, L hand represents the distance from the center of the human wrist joint to the hand, and L represents the distance from the center of the human elbow joint to the wrist joint;

[0144] By transforming the inverse kinematics problem into a multi-objective constrained optimization problem, the LM iteration method is applied to find a solution. This method ensures that the obtained inverse solution not only satisfies all constraints but is also the only definite solution under these constraints, eliminating the uncertainty and randomness of the solution. The optimization model is as follows:

[0145]

[0146] The above formula represents the objective function of the inverse kinematics problem of the manipulator, which includes two nonlinear equations and one constraint equation;

[0147] The S513 includes the following steps:

[0148] S5131, setting the pose matrix of the end effector of the human arm; substituting the current joint angle of the robotic arm into the pose transformation matrix of the end effector relative to the base coordinate system to obtain the current pose matrix of the end effector of the robotic arm;

[0149] S5132, calculating the position error of the end effector of the robotic arm according to the pose matrix of the end effector of the human arm and the current pose matrix of the end effector of the robotic arm;

[0150] S52, using an optical motion capture system to obtain trajectory data of the end of the robotic arm during the human-machine motion model conversion operation; comparing the trajectory data of the end of the robotic arm with the trajectory data of the end of the human arm to obtain a reproduction accuracy evaluation result of the robotic arm motion;

[0151] Experimental conditions:

[0152] Reference Figure 7 、 Figure 8 As shown, in order to verify the feasibility and accuracy of the human-machine motion mapping system proposed in this solution, this embodiment records a human arm swinging motion experiment; reflective markers are attached to each joint of the arm; the coordinate data of each reflective marker is collected by the eight-camera Mar2H optical motion capture system of Zhizhu Technology, and the angle change of the wrist joint during the motion is calculated. The joint angle of the robotic arm is calculated using the iterative algorithm proposed in this article, and then the obtained robotic arm joint angle is transmitted to the Ruiman RM65-B six-degree-of-freedom robotic arm to achieve trajectory reproduction, and the trajectory of the end of the robotic arm is captured by the optical motion capture device at the same time; in the experiment, the experimenter repeated the experimental action 10 times and collected motion data, and recorded the entire experimental process with a camera;

[0153] Experimental verification:

[0154] (1) Motion data collection:

[0155] Reference Figure 9 、 Figure 10 、 Figure 11 、 Figure 12 As shown, in the experiment, the optical motion capture system outputs data in real time, which includes displacement, velocity and acceleration data in the world coordinate system;

[0156] (2) Data preprocessing:

[0157] Reference Figure 13 、 Figure 14 As shown in the figure, in order to further improve the accuracy of motion reproduction and eliminate the problems of noise, error and incompleteness in the original data, the positional relationship between the visible points and the missing points is first used to fill the data using cubic interpolation. In order to solve the cubic spline interpolation, it is usually necessary to construct a linear equation system. The coefficients of this equation system are determined by the values ​​of the data points and the endpoint conditions. Then, the Gaussian elimination method can be used to solve this equation system to obtain the coefficients of each piecewise cubic polynomial. This provides a smooth curve while maintaining the interpolation accuracy of the data points.

[0158] Reference Figure 15 、 Figure 16As shown in the figure, in order to eliminate the noise in the original data caused by the operator's hand shaking during movement, the Savitzky-Golay five-point cubic filtering method is used to remove the noise in the motion trajectory. In the time domain, based on a second-order polynomial, the least squares method is used to perform the best fit through a moving window, and the average error is 0.0919mm, which meets the purpose of SG filtering to make the trajectory smooth but not deviate from the original data.

[0159] (3) Iterative calculation of joint angles:

[0160] Using the end coordinate position after data preprocessing, the corresponding arm joint angle is obtained through the geometric solution in inverse kinematics. Then, the joint angle data of the robot arm is obtained by using the inverse kinematics iterative algorithm, as shown in Table 2.

[0161] Table 2 Iterative joint angles of the robotic arm

[0162]

[0163]

[0164] (4) Experimental results:

[0165] In the robot arm following experiment, the trajectory comparison of the arm and the end of the robot arm is shown in the figure below. Figure 17 、 Figure 18 、 Figure 19 、 Figure 20 As shown; Analysis of the trajectory shows that the robot may have no solution in the process of iterative inverse kinematics solution. This paper uses the cosine similarity comparison method to compare the two trajectory curves. The higher the overlap of the two trajectories, the closer the cosine similarity value is to 1. Through the calculation of the experimental results, the similarity of the trajectory curves obtained under the premise of taking into account the end accuracy and joint anthropomorphism is 0.7821, of which the similarity of the projection curve on the xy plane is 0.7841, the similarity of the projection curve on the xz plane is 0.7931, and the similarity of the projection curve on the yz plane is 0.5126; there is a large gap with the ideal similarity. The reason for this phenomenon may be the diameter of the reflective mark point at the end, and the friction between the joints of the robot arm leads to the cumulative error of the end; Table 3 shows the comparison of the iteration time of the LM-GA algorithm proposed in this paper and the single LM algorithm. The LM-GA algorithm proposed in this paper reduces the iteration time by 12.05%, greatly improving the iteration efficiency;

[0166]

[0167] Where CosineSimilarity represents the cosine similarity between the trajectory of the end-arm and the trajectory of the end-effector of the robot arm, θ represents the angle between the trajectory of the end-arm and the trajectory of the end-effector of the robot arm, A represents the three-dimensional coordinate matrix of the trajectory of the end-arm, and B represents the three-dimensional coordinate matrix of the trajectory of the end-effector of the robot arm.

[0168] Table 3 Comparison of algorithm time

[0169]

[0170] Example 2

[0171] This embodiment discloses a human-machine motion mapping control system based on a hybrid LM-GA algorithm. The system can implement the method of the above embodiment, including an arm joint position information data acquisition module, a preprocessing module, an arm joint angle data acquisition module, a robot arm joint angle data calculation module, a robot arm end trajectory data acquisition module, and a robot arm motion accuracy assessment module.

[0172] The arm joint position information data acquisition module is used to adjust the positions and angles of multiple cameras of the optical motion capture system; after the adjustment is completed, the optical motion capture system is used to collect the arm joint position information data of the experimenter during movement to obtain the original arm joint position information;

[0173] The preprocessing module is used to preprocess the original arm joint position information to obtain processed arm joint position information;

[0174] The arm joint rotation angle data acquisition module is used to obtain the arm joint rotation angle data corresponding to the processed arm joint position information by using a geometric solution in inverse kinematics;

[0175] The robotic arm joint angle data calculation module is used to calculate the arm joint angle data using an inverse kinematics iterative algorithm to obtain the robotic arm joint angle data;

[0176] The robot arm end trajectory data acquisition module is used to verify the joint angle data of the robot arm in a virtual environment; during the verification process, an optical motion capture system is used to obtain the trajectory data of the robot arm end;

[0177] The robot arm motion accuracy assessment module is used to compare the trajectory data of the end of the robot arm with the trajectory data of the end of the human arm to obtain a reproducible accuracy assessment result of the robot arm motion.

[0178] Throughout this specification, references to terms such as "one embodiment," "example," or "specific example" indicate that the specific features, structures, materials, or characteristics described in conjunction with that embodiment or example are included in at least one embodiment or example of the invention. In this specification, schematic representations of these terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.

[0179] The preferred embodiments of the invention disclosed above are intended only to help illustrate the invention. These preferred embodiments do not exhaust all details, nor do they limit the invention to the specific embodiments described. Obviously, many modifications and variations are possible based on the content of this specification. These embodiments are selected and described in detail in this specification to better explain the principles and practical applications of the invention, thereby enabling those skilled in the art to better understand and utilize the invention.

Claims

1. A human-machine motion mapping control method based on a hybrid LM-GA algorithm, characterized in that: The following steps are involved: S1. Adjusting the positions and angles of multiple cameras of the optical motion capture system; After the adjustment is completed, the optical motion capture system is used to collect the experimenter's arm joint position information data to obtain the original arm joint position information; S2. Preprocessing the original arm joint position information to obtain processed arm joint position information; S3. Using a geometric solution in inverse kinematics to obtain arm joint angle data corresponding to the processed arm joint position information; S4. Calculating the arm joint angle data using an inverse kinematics iterative algorithm to obtain the joint angle data of the robotic arm; S5. performing a human-machine motion model conversion operation in conjunction with an iterative numerical solution of a hybrid LM-GA and verifying the joint angle data of the robotic arm in a virtual environment; During the verification process, an optical motion capture system is used to obtain the trajectory data of the end of the robotic arm; Comparing the trajectory data of the end of the robotic arm with the trajectory data of the end of the human arm to obtain a reproducibility accuracy evaluation result of the robotic arm movement; S41. Using the improved DH parameter method, the geometric relationship between the joints of the robotic arm and the position and posture of the end effector are modeled to obtain a robotic arm model and an end effector model; S51, performing a human-machine motion model conversion operation based on the robot arm model and the end effector model and using an iterative numerical solution of a hybrid LM-GA; The S51 includes the following steps: S511, encoding the joint angle to obtain the joint angle code; constructing a chromosome population and initializing the chromosome population according to the joint angle code; S512, using a fitness function to calculate the fitness of each chromosome in the chromosome population to obtain a fitness value set; selecting individuals from the chromosome population according to the fitness value set to obtain selected individuals; when the selected individuals meet the requirements, using the selected individuals as initial values ​​of the LM algorithm; Otherwise, perform individual crossover and individual mutation operations on the chromosome population, and repeat S512 until the selected individuals meet the requirements; S513, constructing an objective function; initializing the parameters of the LM algorithm and calculating the error of the objective function; when the error of the objective function meets the requirement, the algorithm ends; otherwise, re-initializing the parameters of the LM algorithm and repeating S513 until the error of the objective function meets the requirement; The S513 includes the following steps: S5131, setting the pose matrix of the end effector of the human arm; substituting the current joint angle of the robotic arm into the pose transformation matrix of the end effector relative to the base coordinate system to obtain the current pose matrix of the end effector of the robotic arm; S5132. Calculate the position error of the end of the robotic arm based on the pose matrix of the end of the human arm and the current pose matrix of the end effector of the robotic arm.

2. The human-machine motion mapping control method based on the hybrid LM-GA algorithm according to claim 1, characterized in that: The original arm joint position information in S1 includes displacement, velocity and acceleration data in the world coordinate system.

3. The human-machine motion mapping control method based on the hybrid LM-GA algorithm according to claim 2 is characterized in that: The S2 comprises the following steps: S21, performing data filling using cubic interpolation according to the positional relationship between the visible points and the missing points in the original arm joint position information to obtain filled data; S22. Use Savitzky-Golay five-point cubic filtering to remove noise in the motion trajectory of the padded data to obtain processed arm joint position information.

4. The human-machine motion mapping control method based on the hybrid LM-GA algorithm according to claim 1 is characterized in that: The S41 includes the following steps: S411, set the robot arm joint set; define parameters for each robot arm joint in the robot arm joint set to obtain a robot arm joint parameter matrix ; S412, according to the robot arm joint parameter matrix Calculate the number of robot arm joints i -1 robot arm joint coordinate system to the i The transformation matrix of the robot arm joint coordinate system ; Calculate the pose transformation matrix of the end effector relative to the base coordinate system .

5. A system for implementing the human-machine motion mapping control method based on the hybrid LM-GA algorithm according to any one of claims 1 to 4, comprising an arm joint position information data acquisition module, a preprocessing module, an arm joint angle data acquisition module, a manipulator joint angle data calculation module, a manipulator end trajectory data acquisition module, and a manipulator motion accuracy assessment module; The arm joint position information data acquisition module is used to adjust the positions and angles of multiple cameras of the optical motion capture system; After the adjustment is completed, the optical motion capture system is used to collect the arm joint position information data of the experimenter during movement to obtain the original arm joint position information; The preprocessing module is used to preprocess the original arm joint position information to obtain processed arm joint position information; The arm joint rotation angle data acquisition module is used to obtain the arm joint rotation angle data corresponding to the processed arm joint position information by using a geometric solution in inverse kinematics; The robotic arm joint angle data calculation module is used to calculate the arm joint angle data using an inverse kinematics iterative algorithm to obtain the robotic arm joint angle data; The robot arm end trajectory data acquisition module is used to verify the joint angle data of the robot arm in a virtual environment; During the verification process, an optical motion capture system is used to obtain the trajectory data of the end of the robotic arm; The robot arm motion accuracy assessment module is used to compare the trajectory data of the end of the robot arm with the trajectory data of the end of the human arm to obtain a reproducible accuracy assessment result of the robot arm motion.

Images