Human-machine action mapping control system and method based on hybrid LM-GA algorithm

Abstract

The invention discloses a human-machine motion mapping control system and method based on a hybrid LM-GA algorithm, and relates to the field of human-machine motion mapping control. According to arm joint position information obtained by an optical motion capture system, a corresponding arm joint rotation angle is obtained by using a geometric solution in inverse kinematics; secondly, joint angle data of the mechanical arm are obtained through an inverse kinematics iterative algorithm, and the joint angle data are verified in the virtual environment; then track data of the tail end of the mechanical arm is obtained through an optical motion capture system, the motion reproduction precision of the mechanical arm can be evaluated by comparing the track data of the tail end of the human arm and the track data of the tail end of the mechanical arm, and the accuracy and reliability of a human-machine motion mapping system are ensured; 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 motions and converts them into machine commands or control signals to achieve seamless communication between humans and machines. This technology is widely used in virtual reality, robot control, game interaction, sports rehabilitation and other fields.

[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 high 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 above-mentioned high-precision assembly system has shown excellent performance and precision in the field of micro-assembly, but the design complexity of the system may lead to high costs and maintenance requirements, and regular calibration is required to ensure continued high-precision performance. In addition, the adaptation of the system to specific parts may require specific grippers and adjustments, which to a certain extent limits its versatility. With the improvement of assembly technology and process standardization, the application of robots in the 3C industry has grown rapidly, and there is a strong demand for automation transformation. Although modern robotic arms have high repeatability, they are not accurate enough for certain assembly tasks in the 3C industry that require high precision. At the same time, current visual systems and sensor technologies are still limited in handling complex scenes and tiny objects, and there are also problems with delays and trajectory confusion. Summary of the invention

[0005] In view of 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-mentioned technical problems existing in the existing related art.

[0006] In order 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, using an inverse kinematics iterative algorithm to calculate the arm joint angle data 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; using an optical motion capture system to obtain the trajectory data of the end of the robotic arm during the verification process; 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 evaluation result of the robotic arm motion;

[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 necessary to first adjust the position and angle of each camera to ensure that they can 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, the occlusion of the markers, and even the change of ambient light; these problems may lead to data errors, noise, and even data incompleteness; therefore , it is particularly important to preprocess these raw data effectively. At the same time, different effects of preprocessing 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, according to the arm joint position information obtained by the optical motion capture system, the corresponding arm joint angles are obtained by using the geometric solution in inverse kinematics, and then the joint angle data of the robot arm is obtained by using the inverse kinematics iterative algorithm and verified in a virtual environment first; then the optical motion capture system is used to obtain the trajectory data of the end of the robot arm. By comparing the trajectory data of the end of the human arm and the end of the robot arm, the reproduction accuracy of the robot 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, S2 comprises the following steps:

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

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

[0018] In order to further improve the accuracy of motion reproduction and eliminate the problems of noise, error and incompleteness that may exist in the original data, the positional relationship between the visible points and the lost 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 which are determined by the values ​​of the data points and the endpoint conditions. Then, the Gaussian elimination method can be used to solve the equation system to obtain the coefficients of each piecewise cubic polynomial, which provides a smooth curve while maintaining the interpolation accuracy of the data points. In order to eliminate the noise in the original data caused by the jitter of the operator's hand during movement, the Savitzky-Golay five-point cubic filtering method is used to remove the noise in the motion trajectory. Based on the second-order polynomial in the time domain, the least squares method is used to perform the best fit through the moving window, which meets the purpose of SG filtering to make the trajectory smooth but not deviate from the original data.

[0019] Preferably, S4 comprises the following steps:

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

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

[0022] S411, set the robot joint set a = {a1, a2, a3, a4, a5, a6}, a1, a2, a3, a4, a5, a6 represent the first joint, the second joint, the third joint, the fourth joint, the fifth joint and the sixth joint of the robot respectively; define parameters for each robot joint in the robot joint set to obtain a robot 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 respectively represents the joint angle parameter of the i-th mechanical arm joint in the mechanical arm 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 mechanical arm joint coordinate system to the i-th mechanical arm joint coordinate system in the mechanical arm joint set according to the mechanical arm 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, S5 comprises the following steps:

[0030] S51, performing a human-machine motion model conversion operation according to the robot arm model and the end effector model and using an iterative numerical solution method 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 reproducibility 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 an accurate end effector trajectory for the robot, but also can further calculate the real-time angle value of the wrist joint of the arm through spatial geometric analysis. The wrist joint angle value is used as a constraint condition for the joint motion of the robot to ensure that the movement of the robot conforms to the anthropomorphic trajectory while maintaining efficiency and accuracy. 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. Inverse kinematics is the process of determining the angles of each joint of the robot 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 joints until the predetermined end effector position is reached.

[0033] Preferably, the S51 comprises 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 the initial values ​​of the LM algorithm;

[0036] Otherwise, perform individual crossover and individual mutation operations on the chromosome population, and repeat S512 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, which takes a positive value when the joint angle exceeds the limit and is zero otherwise; ω1 and ω2 both represent 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 posture matrix of the end of the human arm; substituting the current joint angle of the robot arm into the posture transformation matrix of the end effector relative to the base coordinate system to obtain the current posture matrix of the end effector of the robot arm;

[0042] S5132, calculating the position error of the end of the robotic arm according to the pose matrix of the end 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 continues to approach 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, and 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, comprising an arm joint position information data acquisition module, a preprocessing module, an arm joint angle data acquisition module, a mechanical arm joint angle data calculation module, a mechanical arm end trajectory data acquisition module and a mechanical 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 the 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 the geometric solution in inverse kinematics;

[0048] The robot arm joint angle data calculation module is used to calculate the arm joint angle data using an inverse kinematics iterative algorithm to obtain the robot 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 is obtained according to the optical motion capture system, and the corresponding arm joint angle is obtained by using the geometric solution in inverse kinematics. Secondly, the inverse kinematics iterative algorithm is used to obtain the joint angle data of the robot 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 robot arm. By comparing the trajectory data of the end of the human arm and the end of the robot arm, the reproduction accuracy of the robot 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 points and lost points and adopts cubic interpolation to fill in data, thereby obtaining the coefficients of each piecewise cubic polynomial, and providing a smooth curve while maintaining the interpolation accuracy of the data points. The Savitzky-Golay five-point cubic filtering method is used to remove noise from the motion trajectory, and the least squares method is used to perform the best fit through a moving window based on a second-order polynomial in the time domain, thereby satisfying the purpose of SG filtering to make the trajectory smooth without deviating from the original data.

[0054] 3. The present invention adopts an improved iterative algorithm 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.

[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 drawings required for describing the embodiments will be briefly introduced below. 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 creative work.

[0057] Figure 1 A 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 robot 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 It is a corresponding diagram of the joints of the arm of the present invention and the mechanical 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 It is the 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 It is the action posture diagram of the arm experiment of the present invention;

[0065] Fig. 9 A data graph of the optical motion capture system of the present invention;

[0066] Fig.10 Schematic diagram of position coordinates in the time series data sample of the experimental action of the present invention, wherein 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] Fig.11 Schematic diagram of velocity coordinates in the time series data sample of the experimental action of the present invention, wherein 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] Fig.12 is a schematic diagram of 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] Fig.13 This is a schematic diagram of data before cubic interpolation of the present invention;

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

[0071] Fig.15 This is a comparison diagram 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] Fig.16 This is a schematic diagram of the SG filtering error of the present invention;

[0073] Fig.17 It is the trajectory of the end effector of the arm in three-dimensional space of the present invention;

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

[0075] Fig.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] Fig. 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] Fig.21 It 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 be combined with the drawings in the embodiments of the invention to clearly and completely describe the technical solutions in the embodiments of the invention. Obviously, the described embodiments are only part of the embodiments of the invention, not all of the embodiments. Based on the embodiments in the invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the invention.

[0079] In the description of the present invention, it is necessary to understand 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] Embodiment 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, filling data using cubic interpolation according to the positional relationship between the visible points and the lost points in the original arm joint position information to obtain filled data;

[0086] S22, using a Savitzky-Golay five-point 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, using an inverse kinematics iterative algorithm to calculate the arm joint angle data to obtain the joint angle data of the robotic arm;

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

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

[0091] Due to the differences in kinematics and dynamics between human arms and robotic arms, it is necessary to deeply analyze the joint structures of the two. Based on the theory of robotics, 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 pronation / supination rotation ability of the forearm, which work together to form a ball-joint-like structure. This structure allows the human arm to show a high degree of flexibility and dynamic adjustment ability when performing tasks.

[0092] The human arm joints are driven by bones and muscles. Specifically, when the human shoulder joint moves, although it looks like a single movement as a whole, 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 is responsible for changing the position, and the other around the axis is responsible for adjusting the posture. The 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 be as flexible as the human arm. It is quite different from the human arm during movement, which brings difficulties to the mapping between the human arm and the robotic arm.

[0093] The S41 comprises the following steps:

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

[0095]

[0096] Among them, ai '1, a i ′2, a i '3, a i '4, a i ′5 respectively represents the joint angle parameter of the i-th mechanical arm joint in the mechanical arm 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 mechanical arm joint coordinate system to the i-th mechanical arm joint coordinate system in the mechanical arm joint set according to the mechanical arm 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; using an optical motion capture system to obtain the trajectory data of the end of the robotic arm during the verification process; 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 evaluation result of the robotic arm motion;

[0103] The S5 comprises the following steps:

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

[0105] The S51 comprises 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 the initial values ​​of the LM algorithm;

[0108] Otherwise, perform individual crossover and individual mutation operations on the chromosome population, and repeat S512 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, which takes a positive value when the joint angle exceeds the limit and is zero otherwise; ω1 and ω2 both represent 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 approach 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, and the optimal solution that meets the conditions is obtained after multiple iterative calculations;

[0114] Iteratively solve the objective function; The iterative mapping algorithm deals with the robot inverse kinematics problem 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 end effector and the predetermined target; Specifically, the construction of the objective function follows 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 vector 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 posture 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 vector of the end of the robot 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 relative to the base coordinate system, p r Represents the position vector of the end of the robot relative to the base coordinate system;

[0121] In order to make the end effector of the robot 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 of the robot arm can be approximately expressed as:

[0122]

[0123] In the formula, Δθ 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 θ 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 robot 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. When solving Δδ, a generalized inverse matrix is ​​needed. The matrix J θ The generalized inverse matrix of is The solution to the linear equations is:

[0132]

[0133] The algorithm realizes the convergence of dynamically adjustable iteration step size by introducing the adjustment of 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 is constantly close to 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] Iterative solution of joint constraints; Considering the above iterative objective function, there are infinite solutions due to the lack of end posture constraints, and the rotation angle transformation of the end effector from the current posture to the target posture 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 iteration method of the robot, 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 motion, 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 robot 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] In order to realize the anthropomorphic movement of the robotic arm, the real-time change value of the angle of the wrist joint of the human arm is transmitted to the corresponding joint of the robotic arm; specifically, an optical motion capture device is used to set three reflective marker points on the arm, located at the wrist joint, elbow joint and hand, to construct a geometric structure of an "arm triangle"; the coordinate position of the reflective marker point can accurately reflect the angle change of the wrist joint, and give 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 converting 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 equation represents the objective function of the inverse kinematics problem of the robot arm, which contains two nonlinear equations and one constraint equation;

[0147] The S513 comprises the following steps:

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

[0149] S5132, calculating the position error of the end of the robotic arm according to the pose matrix of the end 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 reproducibility 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 scheme, this embodiment records a human motion experiment of arm swinging; reflective marking points are pasted on each joint of the arm; the coordinate data of each reflective marking point is collected by the eight-camera Mar2H optical motion capture system of Metrics Technology, and the angle change of the wrist joint during the movement is calculated. The joint angle of the robot arm is calculated using the iterative algorithm proposed in this article, and then the obtained joint angle of the robot arm is transmitted to the six-degree-of-freedom robot arm of the Ruiman RM65-B model to realize trajectory reproduction, and the trajectory of the end of the robot 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 Fig. 9 , Fig.10 , Fig.11 , Fig.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 Fig.13 , Fig.14 As shown, in order to further improve the accuracy of action reproduction and eliminate the problems of noise, error and incompleteness that may exist in the original data, the positional relationship between the visible points and the lost 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 which are determined by the values ​​of the data points and the endpoint conditions, and then the Gaussian elimination method can be used to solve the equation system to obtain the coefficients of each piecewise cubic polynomial; while maintaining the interpolation accuracy of the data points, a smooth curve is provided;

[0158] Reference Fig.15 , Fig.16As shown in the figure, in order to eliminate the noise in the original data caused by the shaking of the operator's hand 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 the second-order polynomial, the least squares method is used to perform the best fit through the 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, and then the inverse kinematics iterative algorithm is used to obtain the joint angle data of the robot arm, as shown in Table 2;

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

[0162]

[0163]

[0164] (4) Experimental results:

[0165] In the robot following experiment, the trajectory comparison of the arm and the end of the robot is shown in the figure below. Fig.17 , Fig.18 , Fig.19 , Fig. 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 precision and joint anthropomorphism is 0.7821, among 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; the gap with the ideal similarity is large. The reason for this phenomenon may be the diameter of the reflective mark point at the end. At the same time, 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%, which greatly improves the iteration efficiency;

[0166]

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

[0168] Table 3 Comparison of algorithm time

[0169]

[0170] Embodiment 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 mechanical arm joint angle data calculation module, a mechanical arm end trajectory data acquisition module and a mechanical arm motion accuracy evaluation 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 the 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 the geometric solution in inverse kinematics;

[0175] The robot arm joint angle data calculation module is used to calculate the arm joint angle data using an inverse kinematics iterative algorithm to obtain the robot 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] In the description of this specification, the description with reference to the terms "one embodiment", "example", "specific example", etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0179] The preferred embodiments of the invention disclosed above are only used to help explain the invention. The preferred embodiments do not describe all the details in detail, nor do they limit the invention to the specific implementation methods described. Obviously, many modifications and changes can be made according to the content of this specification. This specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the invention, so that those skilled in the art can understand and use the invention well.

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, using an inverse kinematics iterative algorithm to calculate the arm joint angle data to obtain the joint angle data of the robotic arm; S5, performing a human-machine motion model conversion operation in cooperation 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; the trajectory data of the end of the robotic arm is compared with the trajectory data of the end of the human arm to obtain the reproduction accuracy evaluation result of the robotic arm movement.

2. The human-machine motion mapping control method based on the hybrid LM-GA algorithm according to claim 1 is 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, filling data using cubic interpolation according to the positional relationship between the visible points and the lost points in the original arm joint position information to obtain filled data; S22. Use the Savitzky-Golay five-point cubic filtering method to remove the noise in the motion trajectory of the padded data to obtain the processed arm joint position information.

4. The human-machine motion mapping control method based on the hybrid LM-GA algorithm according to claim 3 is characterized in that: The S4 comprises the following steps: S41. Use the improved DH parameter method to model the geometric relationship between the joints of the robot arm and the position and posture of the end effector to obtain the robot arm model and the end effector model.

5. The human-machine motion mapping control method based on the hybrid LM-GA algorithm according to claim 4 is characterized in that: The S41 comprises the following steps: S411, setting a robotic arm joint set; defining parameters for each robotic arm joint in the robotic arm joint set to obtain a robotic arm joint parameter matrix a′; S412, calculating the transformation matrix of the transformation from the i-1th mechanical arm joint coordinate system to the i-th mechanical arm joint coordinate system in the mechanical arm joint set according to the mechanical arm joint parameter matrix a′ Calculate the pose transformation matrix of the end effector relative to the base coordinate system 6. The human-machine motion mapping control method based on the hybrid LM-GA algorithm according to claim 5 is characterized in that: The S5 comprises the following steps: S51, performing a human-machine motion model conversion operation according to the robot arm model and the end effector model and using an iterative numerical solution method of a hybrid LM-GA; S52. During the human-machine motion model conversion operation, an optical motion capture system is used to obtain trajectory data of the end of the robotic arm; the trajectory data of the end of the robotic arm is compared with the trajectory data of the end of the human arm to obtain a reproduction accuracy evaluation result of the robotic arm motion.

7. The human-machine motion mapping control method based on the hybrid LM-GA algorithm according to claim 6 is characterized in that: The S51 comprises 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 the 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 requirements, the algorithm ends; otherwise, re-initializing the parameters of the LM algorithm and repeating S513 until the error of the objective function meets the requirements.

8. The human-machine motion mapping control method based on the hybrid LM-GA algorithm according to claim 7 is characterized in that: The S513 comprises the following steps: S5131, setting the posture matrix of the end of the human arm; substituting the current joint angle of the robot arm into the posture transformation matrix of the end effector relative to the base coordinate system to obtain the current posture matrix of the end effector of the robot arm; S5132. Calculate the position error of the end of the robotic arm according to the pose matrix of the end of the human arm and the current pose matrix of the end effector of the robotic arm.

9. A system for implementing the human-machine motion mapping control method based on the hybrid LM-GA algorithm as described in any one of claims 1 to 8, comprising an arm joint position information data acquisition module, a preprocessing module, an arm joint angle data acquisition module, a mechanical arm joint angle data calculation module, a mechanical arm end trajectory data acquisition module and a mechanical arm motion accuracy evaluation 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 the 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 the geometric solution in inverse kinematics; The robot arm joint angle data calculation module is used to calculate the arm joint angle data using an inverse kinematics iterative algorithm to obtain the robot 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 was used to obtain trajectory data at 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