Dual-arm robot anthropomorphic joint posture mapping method based on particle swarm optimization algorithm

Abstract

The present application relates to a kind of double-arm robot anthropomorphic joint posture mapping method based on particle swarm optimization algorithm, belong to the field of robot kinematics and trajectory planning, its main content is, the unified representation method of human double-arm joint posture and robot double-arm joint posture is established, i.e.spherical coordinate representation method.Equivalent to three sequentially connected vectors of human upper arm, forearm and palm joint, equivalent to upper arm forearm and palm joint of mechanical arm, each joint is regarded as a vector, three vectors are sequentially connected, the method of the patent uses the representation method of spherical coordinate to determine the spatial position of human arm three joint vectors and mechanical arm three joint vectors, through anthropomorphic joint posture mapping algorithm, so that the polar coordinate and azimuth angle parameters of double-arm robot equivalent joint vector and human arm joint vector are equal, then the joint angle value of mechanical arm that meets the condition is obtained by particle swarm optimization algorithm, so as to realize the migration from human arm joint posture to double-arm robot anthropomorphic posture.

Description

Technical Field

[0001] This invention belongs to the field of robot kinematics and trajectory planning, and specifically relates to a method for mapping anthropomorphic joint postures of a dual-arm robot based on particle swarm optimization algorithm. Background Technology

[0002] With the development of robot motion control and trajectory planning technologies, humans have endowed dual-arm robots with a variety of functions. A key technological challenge in enabling dual-arm robots to collaborate efficiently, accurately, and naturally with humans or mimic human operations lies in achieving anthropomorphic joint posture mapping. Dual-arm robots are increasingly widely used in modern manufacturing, logistics, medical rehabilitation, and service industries, demonstrating enormous potential in improving production efficiency, enhancing operational flexibility, and achieving human-robot collaboration. Enabling dual-arm robots to exhibit the same movement postures as human hands during task execution can significantly increase their approachability and the interpretability of their movements, making them easier to integrate into people's lives. More precisely, in traffic control robots performing traffic management tasks, the posture of the robot's arms is the most important traffic signal; therefore, it is particularly important for the robot to accurately mimic the arm postures of traffic police officers during task execution.

[0003] In recent years, a large number of deep learning-based humanoid joint pose mapping techniques for bi-armed robots have emerged. Firstly, this method requires researchers to have strong background knowledge in computer science and artificial intelligence. When using deep learning to achieve robot joint pose mapping, a large amount of training data is needed to learn effective feature representations and mapping relationships. Moreover, deep learning models (such as deep neural networks) have complex structures and a large number of parameters, and their internal decision-making processes are often difficult to understand and interpret. The training and inference processes of deep learning models typically require significant computational resources, including high-performance graphics processing units.

[0004] Secondly, reinforcement learning algorithms typically require robots to undergo extensive trial-and-error exploration in their environment to learn the optimal policy. This trial-and-error process is often very time-consuming in practical applications. Furthermore, reinforcement learning algorithms are prone to getting trapped in local optima during the search for the optimal policy, especially in complex high-dimensional motion spaces (such as the joint pose space of a dual-arm robot), where numerous local optima exist. Therefore, a new mapping algorithm is urgently needed to address these issues. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a humanoid joint pose mapping method for dual-arm robots based on particle swarm optimization. This method equates the human upper arm, forearm, and hand joints to three sequentially connected vectors, and the robotic arm to the upper arm, forearm, and hand joints. Each joint is treated as a vector, and the three vectors are sequentially connected. Then, spherical coordinates are used to determine the spatial positions of the three joint vectors of the human arm and the robotic arm. The humanoid joint pose mapping algorithm ensures that the polar coordinates and azimuth parameters of the equivalent joint vectors of the dual-arm robot are equal to those of the human arm joint vectors. Finally, the particle swarm optimization algorithm is used to obtain the robotic arm joint angle values ​​that meet the conditions, thereby realizing the transfer from the human arm joint pose to the humanoid pose of the dual-arm robot.

[0006] To achieve the above objectives, the present invention provides the following technical solution:

[0007] This invention provides a method for anthropomorphic joint pose mapping of a dual-arm robot based on particle swarm optimization algorithm, which includes the following steps:

[0008] S1. Feasibility analysis of anthropomorphic posture motion of dual-arm robot;

[0009] S2. Establish the kinematic model of the robotic arm based on the DH parameters of the robotic arm, and use the forward kinematic formula to calculate the position coordinates of the equivalent shoulder, elbow, wrist and palm position coordinate system.

[0010] S3. Obtain motion data of the joints of both arms from the database, extract the position coordinates of the shoulder, elbow, wrist and palm joints from the motion data of the joints of both arms, and calculate the vector coordinate values ​​of the upper arm, forearm and palm.

[0011] S4. Calculate the spherical coordinates of the vector coordinates of the upper arm, forearm, and palm using spherical coordinate representation, i.e., the target spherical coordinate parameter values ​​(θ, φ, r) of the arm joints; and represent the target spherical coordinate parameter values ​​(θ, φ, r) of the arm joints as the spherical coordinates of the equivalent joints of the robotic arm, thereby obtaining the target spherical coordinate values ​​of the robotic arm, and calculating the position coordinates of the target joints of the robotic arm, obtaining the target coordinates of the elbow, wrist, and palm of the robotic arm, represented as D1_target, D2_target, and D3_target, respectively.

[0012] S5. Use the particle swarm optimization algorithm to obtain the joint angle values ​​of the robotic arm that meet the constraints, calculate the corresponding joint angles frame by frame and save them, obtain the frame-by-frame joint angle values ​​of the robotic arm to complete the entire movement and send them to the robotic arm.

[0013] Preferably, step S5 specifically includes the following sub-steps:

[0014] S51. Construct joint constraint equations:

[0015] qi lower ≤qi≤qi upper ;

[0016] Where qi is the joint angle of the robotic arm, qi lower Qi is the minimum joint angle of the robotic arm. upper This represents the maximum joint angle of the robotic arm;

[0017] S52. Randomly initialize joint angle values ​​and iterate to calculate the transformation matrix from the base coordinate system of the robotic arm to the equivalent joint coordinate system.

[0018] S53. Calculate the position coordinates of the transformation matrix according to the DH parameters of the robotic arm to obtain the actual coordinates of the equivalent joints of the robotic arm, namely elbow, wrist and palm, which are represented as P_3, P_5 and P_7 respectively.

[0019] S54. By subtracting the actual coordinates of the equivalent joints of the robotic arm from the target coordinates of the elbow, wrist, and palm, and using the mean square error norm of the difference vector as the error vector to be optimized, the fitness function of the particle swarm optimization algorithm is defined as fitness=norm(e1)+norm(e2)+norm(e3).

[0020] The error vector is represented using mean square error, specifically:

[0021] e1=P_3-D1_target, e2=P_5-D2_target, e3=P_7-D3_target;

[0022] Where norm is the Euclidean norm, and e1, e2, and e3 are the error vectors of the joints elbow, wrist, and palm, respectively.

[0023] S55. Filter the motion data of the joints of the two arms, and save the processed motion sequence by calculating the corresponding joint angles frame by frame using steps S51-S54. The frame-by-frame joint angle values ​​of the robotic arm completing the entire motion are obtained and sent to the robotic arm.

[0024] Preferably, in step S52, the transformation matrices from the base coordinate system of the robotic arm to the equivalent joint coordinate system are as follows:

[0025]

[0026] 0 T n = 0 T1× 1 T2× 2 T3×…×n -1 T n

[0027] 0 T3 = 0 T1× 1 T2× 2 T3

[0028] 0 T5 = 0 T1× 1 T2× 2 T3× 3 T4× 4 T5

[0029] 0 T7 = 0 T1× 1 T2× 2 T3× 3 T4× 4 T5× 5 T6× 6 T7;

[0030] Where, d i Let θ be the joint offset of the robotic arm. i For the joint angle, α i-1 For the connecting rod torsion angle, i-1 T i Let be the transformation matrix from the (i-1)th coordinate system of the robotic arm to the ith coordinate system. 0 T3 is the transformation matrix of the robotic arm from the base coordinate system to the third coordinate system. 0 T5 is the transformation matrix of the robotic arm from the base coordinate system to the fifth coordinate system. 0 T n This is the transformation matrix for the robotic arm from the base coordinate system to the n-coordinate system.

[0031] Preferably, in step S54, the position update formula of the particle swarm optimization algorithm is:

[0032] x i (t+1)=x i (t)+v i (t+1), where x i (t) represents the position of particle i at time t, v i (t+1) represents the velocity of particle i at time t_+1;

[0033] In the particle swarm optimization algorithm, the particle velocity update formula is:

[0034] x i (t+1)=x i (t)+v i (t+1)v i (t+1)=w·v i (t)+c1·r1·(pbest i -xi (t))+c2·r2·(gbest-x i (t))

[0035] Among them, v i (t) is the velocity of particle i at time t; w is the inertia weight; c1 and c2 are learning factors; r1 and r2 are uniformly random numbers between (0,1); pbest i The first known historical optimal position of the particle is the individual optimal value; gbest is the global optimal value. After obtaining the new velocity using the above velocity update formula, the new position of the particle at that moment is calculated using the position update formula. The optimal solution is found by iteratively updating the particle's velocity and position.

[0036] Preferably, step S1 specifically involves: according to the spherical coordinate representation of the vector (θ, φ, r), within the joint constraints of the robotic arm, the range of spherical coordinate parameters of the equivalent joints of the robotic arm is obtained through the robot's forward kinematics model; within the joint constraints of the human arm, the range of spherical coordinate parameters of the shoulder, elbow, wrist, and hand joints is solved; the magnitudes of the two are compared, and it is found that the range of parameter values ​​of the equivalent joints of the robotic arm is greater than the range of spherical coordinate parameters of the corresponding joints of the human arm.

[0037] Preferably, step S2 specifically involves: given the DH parameters of the robotic arm, constructing a kinematic model of the dual-arm robot in MATLAB, using forward kinematics to calculate the transformation matrix of the equivalent shoulder, elbow, wrist, and hand positions, and extracting the displacement vector of the transformation matrix, which is the position coordinate of the equivalent joint point of the robotic arm. The joint angle vector of the robotic arm corresponding to this coordinate is the position coordinate of the equivalent shoulder, elbow, wrist, and hand position coordinate system.

[0038] Preferably, step S3 specifically involves: using S... h E h W h H h The four points represent the calculated vector coordinates of the upper arm, forearm, and palm, respectively. h E h W h H h W h H h .

[0039] Preferably, step S4 specifically includes the following sub-steps:

[0040] S41, Set the vector coordinates of the upper arm, forearm, and palm S h E h E h W h W h H hThe spherical coordinates are represented as

[0041] S42. Based on the spherical coordinate representation of the vector coordinates of the upper arm, forearm, and hand, the target spherical coordinate parameter values ​​of the arm joints are expressed as the spherical coordinates of the equivalent joints of the robotic arm. Then, the position coordinates of the target joint vectors of the robotic arm are calculated, and the spherical coordinates of the equivalent joints of the robotic arm are respectively...

[0042] S43. Concatenate the three vectors of the target joint vector position coordinates of the robotic arm in the order of upper arm, forearm and palm to obtain the target coordinates of the robotic arm elbow, wrist and palm.

[0043] Preferably, the dual-arm robot is a seven-degree-of-freedom articulated robot, and the database in step S3 is obtained by collecting and storing the motion data of the joints of the two arms using the NOKOV metric optical 3D motion capture device.

[0044] Preferably, in step S51, the rotation range of joint 1 is ±178°, the rotation range of joint 2 is ±130°, the rotation range of joint 3 is ±178°, the rotation range of joint 4 is ±135°, the rotation range of joint 5 is ±178°, the rotation range of joint 6 is ±128°, and the rotation range of joint 7 is ±360°.

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

[0046] (1) This invention uses vector methods to represent the joint poses of the robotic arm and the human arm. Utilizing the properties of vectors, the equivalent joint poses of the robotic arm and the human arm are equivalently represented as multiple vector segments connected end-to-end. Simultaneously, spherical coordinates are used to unify the representations of the equivalent joint vectors of the robotic arm and the joint vectors of the human arm, building a bridge between their representation methods. Specifically, this invention converts the joint coordinates of the human arm into joint vectors and then into spherical coordinates. The spherical coordinates of the human arm's joint vectors are equivalent to the spherical coordinates of the robotic arm's equivalent joint vectors. The spherical coordinates of the robotic arm's equivalent joint vectors are then converted into rectangular coordinates. The rectangular coordinates of the robotic arm's equivalent joint vectors are used as the target position of the robotic arm's anthropomorphic joint pose, realizing the transfer from the human arm's joint pose to the anthropomorphic pose of the dual-arm robot. The entire process involves minimal computation and ensures the accuracy of the dual-arm robot's anthropomorphic pose.

[0047] (2) The method of the present invention uses optical motion capture equipment to capture the movement of the joints of the human arm and stores the position coordinates of the shoulder, elbow, wrist and palm joints in a database. Then, the motion data of the joints of the two arms is obtained from the database, and the position coordinates of the shoulder, elbow, wrist and palm joints are extracted from the motion data of the joints of the two arms. The vector coordinates of the upper arm, forearm and palm are calculated, which can make the anthropomorphic posture of the two-arm robot more closely match the movement of the joints of the human arm.

[0048] (3) The present invention uses an iterative optimization method, taking the mean square error of the rectangular coordinates of the equivalent joint vector of the robotic arm at the joint angle at the previous moment as the objective function, and using the particle swarm optimization algorithm to solve step by step a set of joint angle values ​​that minimize the objective function. The computation is small and the optimal solution can be obtained quickly. Attached Figure Description

[0049] Figure 1 This is a flowchart illustrating the specific implementation of the present invention;

[0050] Figure 2 This is a comparison diagram of the spherical coordinate parameters of the equivalent joints of the robotic arm of the present invention and the spherical coordinate parameters of the joint vectors of a human arm;

[0051] Figure 3 This is a schematic diagram of the equivalent joint points and joint lengths of the dual-arm robot of the present invention;

[0052] Figure 4 This is a schematic diagram of the joint points and joint vectors of the human arm according to the present invention;

[0053] Figure 5 This is a schematic diagram of the optical motion capture device for capturing the joints of a human arm according to the present invention.

[0054] Figure 6 This is a schematic diagram comparing the joint vectors of the human arm and the equivalent joint vectors of the robotic arm according to the present invention;

[0055] Figure 7 This is a schematic diagram illustrating the process of the particle swarm optimization algorithm of the present invention in finding the global optimum. Detailed Implementation

[0056] Exemplary embodiments, features, and aspects of the present invention will now be described in detail with reference to the accompanying drawings. The same reference numerals in the drawings denote elements that have the same or similar functions. Although various aspects of the embodiments are shown in the drawings, they are not necessarily drawn to scale unless specifically indicated otherwise.

[0057] This invention provides a method for anthropomorphic joint pose mapping of a dual-arm robot based on particle swarm optimization algorithm, such as... Figure 1 As shown, the specific implementation steps are as follows:

[0058] S1. Feasibility analysis of anthropomorphic posture motion of a dual-arm robot. The structural characteristics and dimensions of the robotic arm of a dual-arm robot differ significantly from the joint dimensions and motion mechanisms of a human arm. Therefore, whether the robotic arm can mimic the joint posture characteristics of a human arm needs to be verified beforehand. If the robotic arm can mimic the joint posture of a human arm, then a feasible range of motion needs to be given. If it is feasible within a certain range of motion, then the same posture can be solved in the reachable space. In the unreachable range, joint angle constraints should be imposed, or the topological joint posture can be solved. Therefore, the feasibility of anthropomorphic posture motion of a dual-arm robot needs to be analyzed first, verifying whether the spherical coordinate parameter range of the equivalent joint vectors of the dual-arm robot is greater than the spherical coordinate parameter range of the vectors of the upper arm, forearm, and hand of a human.

[0059] S2. Establish the kinematic model of the robotic arm based on its DH parameters. Calculate the position coordinates of the equivalent shoulder, elbow, wrist, and hand coordinate systems using forward kinematics formulas. Specifically: Given the DH parameters of the robotic arm, construct the kinematic model of the dual-arm robot in MATLAB. Calculate the transformation matrices of the equivalent shoulder, elbow, wrist, and hand positions using forward kinematics. Extract the displacement vectors from the transformation matrices, which are the position coordinates of the equivalent joints of the robotic arm. The joint angle vectors corresponding to these coordinates are the position coordinates of the equivalent shoulder, elbow, wrist, and hand coordinate systems.

[0060] S3. Use the NOKOV metric optical 3D motion capture device to collect and store the motion data of the subject's two arm joints. Retrieve the motion data of the two arm joints from the stored database, extract the position coordinates of the shoulder, elbow, wrist and palm joints from the motion data of the two arm joints, and calculate the vector coordinate values ​​of the upper arm, forearm and palm.

[0061] S4. Using spherical coordinates, calculate the spherical coordinate representation (θ, φ, r) of the vector coordinates of the upper arm, forearm, and hand. Represent the target spherical coordinate parameters of the arm joints as the spherical coordinates of the equivalent joints of the robotic arm, thus obtaining the target spherical coordinate values ​​of the robotic arm. Calculate the position coordinates of the target joints of the robotic arm and obtain the target posture of the robotic arm. Spherical coordinates can uniquely determine the position of a point in space using three parameters. The direction and magnitude of the equivalent joint vector of the robotic arm can be determined using spherical coordinates, as can the direction and magnitude of the joints of the human arm. Represent the target spherical coordinate parameters of the human arm joints as the spherical coordinate parameters of the equivalent joints of the robotic arm, thus obtaining the target spherical coordinate values ​​of the robotic arm, and then calculate the position coordinates of the target joints of the robotic arm.

[0062] Specifically, the shoulder, elbow, wrist, and hand joints and their coordinates are represented by S... h =(x S y S , z S ),

[0063] E h =(x E y E , z E ), W h =(x W y W , z W ), H h =(x H y H , z H The four points represent the joint coordinates obtained through the NOKOV metric optical 3D motion capture device and stored in the database. After extraction, the calculated vectors of the upper arm, forearm, and hand are represented by S. h E h E h W h、 W h H h The details are as follows:

[0064] S h E h =(x E -x S y E -y S , z E -z S E h W h =(x W -x E y W -y E , z W -z E )

[0065] W h H h =(x H -x W y H -y W , z H -z W ).

[0066] Human arm S h E h E h W h、 W h H h The three vectors are represented in spherical coordinates as follows The joint lengths of the arms are ρ1 = 360 mm, ρ2 = 300 mm, and ρ3 = 80 mm.

[0067] The spherical coordinates of the three joint vectors of a human arm are equivalent to the spherical coordinate values ​​of the same equivalent joint vectors of a dual-arm robot, where the equivalent joint points and coordinates of the robotic arm are represented by S. r E r W r H r The equivalent joint vector of the robotic arm is S. r E r E r W r W r H r The spherical coordinate representations of the three equivalent joint vectors are respectively The specific dimensions of r1, r2, and r3 are r1 = 256 mm, r2 = 210 mm, and r3 = 172.5 mm, respectively.

[0068] The spherical coordinate parameters of the obtained human arm joint vectors are matched one-to-one with the equivalent joint spherical coordinate parameters of the robotic arm. However, due to significant differences in the joint lengths and connection methods between the robotic arm's mechanism dimensions and structural configuration and the human arm's, the corresponding equivalent joint lengths differ during the equivalence process. Therefore, the length of the equivalent joint vector is excluded when using spherical coordinate representation. This yields the target coordinates of the robotic arm joint vectors.

[0069] The spherical coordinate representation of the equivalent joint vectors of the robotic arm is converted into rectangular coordinate representation. The shoulder, elbow, wrist, and palm are represented as D0, D1, D2, and D3, respectively. The equivalent shoulder coordinate of the robotic arm is represented as D0 = [0, 24, 0.50]. The calculated position coordinates of the elbow, wrist, and palm of the target robotic arm are as follows:

[0070] D0_target=D0, D1_target=D0+D1, D2_target=D1_target+D2,

[0071] D3_target = D2_target + D3

[0072] The above four coordinates are the target coordinate values ​​of the equivalent joints of the robotic arm. Among them, D0_target is the equivalent shoulder coordinate of the robotic arm, D1_target is the equivalent elbow coordinate of the robotic arm, D2_target is the equivalent wrist coordinate of the robotic arm, and D3_target is the equivalent hand coordinate of the robotic arm.

[0073] S5. Use the particle swarm optimization algorithm to obtain the joint angle values ​​of the robotic arm that satisfy the constraints. The specific steps are as follows:

[0074] S51, The joint constraint equation is: qi lower ≤qi≤qi upperIn this embodiment, the dual-arm robot is a seven-degree-of-freedom articulated robot. The rotation range of joint 1 is ±178°, the rotation range of joint 2 is ±130°, the rotation range of joint 3 is ±178°, the rotation range of joint 4 is ±135°, the rotation range of joint 5 is ±178°, the rotation range of joint 6 is ±128°, and the rotation range of joint 7 is ±360°.

[0075] S52. At the beginning of the iteration, the joint angle values ​​need to be randomly initialized, and the transformation matrix from the base coordinate system of the robotic arm to the equivalent joint coordinate system needs to be calculated, as follows:

[0076]

[0077] 0 T n = 0 T1× 1 T2× 2 T3×…×n -1 T n

[0078] 0 T3 = 0 T1× 1 T2× 2 T3

[0079] 0 T5 = 0 T1× 1 T2× 2 T3× 3 T4× 4 T5

[0080] 0 T7 = 0 T1× 1 T2× 2 T3× 3 T4× 4 T5× 5 T6× 6 T7.

[0081] Where, d i Let θ be the joint offset of the robotic arm. i For the joint angle, α i-1 For the connecting rod torsion angle, i-1 T i Let be the transformation matrix from the (i-1)th coordinate system of the robotic arm to the ith coordinate system. 0 T3 is the transformation matrix of the robotic arm from the base coordinate system to the third coordinate system. 0 T5 is the transformation matrix of the robotic arm from the base coordinate system to the fifth coordinate system. 0 T nThis is the transformation matrix for the robotic arm from the base coordinate system to the n-coordinate system.

[0082] S53. Calculate the position coordinates of the transformation matrix based on the DH parameters of the robotic arm to obtain the actual coordinates of the equivalent joints of the robotic arm: shoulder, elbow, and wrist, denoted as P_3, P_5, and P_7, respectively. In this embodiment, the actual coordinates of the equivalent joints are as follows: 0 T3, 0 T5, The first three rows of the last column are P_3 = S_3.t, P_5 = S_5.t, and P_7 = S_7.t.

[0083] S54. By subtracting the actual coordinates of the equivalent joints from the target posture of the robotic arm's shoulder, elbow, and wrist, the mean square error norm of the difference vector is used as the error vector to be optimized. First, the fitness function of the particle swarm optimization algorithm is defined as fitness = norm(e1) + norm(e2) + norm(e3), where the error vector is represented by the mean square error, specifically:

[0084] e1=P_3-D1_target, e2=P_5-D2_target, e3=P_7-D3_target.

[0085] The position update formula for the particle swarm optimization algorithm is:

[0086] x i (t+1)=x i (t)+v i (t+1), where x i (t) represents the position of particle i at time t, v i (t+1) represents the velocity of particle i at time t_+1.

[0087] In the particle swarm optimization algorithm, the particle velocity update formula is:

[0088] x i (t+1)=x i (t)+v i (t+1)v i (t+1)=w·v i (t)+c1·r1·(pbest i -x i (t))+c2·r2·(gbest-x i (t))

[0089] Among them, v i (t) is the velocity of particle i at time t; w is the inertia weight; c1 and c2 are learning factors; r1 and t2 are uniformly random numbers between (0,1); pbest i`gbest` represents the known historical best position of the first particle, i.e., the individual optimal value; `gbest` represents the known historical best position of the entire particle swarm, i.e., the global optimal value. After obtaining the new velocity using the velocity update formula described above, the new position of the particle at time `gbest` is calculated using the position update formula. The particle swarm optimization algorithm finds the optimal solution by continuously iterating and updating the velocity and position of the particles.

[0090] S55. The joint motion data of the human arm captured by motion capture equipment is smoothed using median filtering to smooth out instability and tremors during the movement. The processed motion sequence is then frame-by-frame, and the corresponding joint angles are calculated and saved to obtain the frame-by-frame joint angle values ​​for the robotic arm to complete the entire movement. The obtained joint angle values ​​are sent to the robotic arm, which can accurately execute the movements demonstrated by the experimenter. The joint pose similarity of the robotic arm movement is evaluated using the minimum value of the fitness function. After solving the problem using the particle swarm optimization algorithm, the minimum error of the fitness function is 0. Therefore, it can be concluded that this human-machine joint pose mapping algorithm can achieve very good mapping results. Specific Implementation

[0092] This invention provides a method for anthropomorphic joint pose mapping of a dual-arm robot based on particle swarm optimization algorithm, such as... Figure 1 As shown, the specific steps are as follows:

[0093] S1. Analyze the feasibility of anthropomorphic posture motion of a dual-arm robot.

[0094] S2. Attach optical marker balls to the shoulder, elbow, wrist, and palm positions of both arms respectively. The capture effect is as follows: Figure 5 As shown, the NOKOV metric optical 3D motion capture device was used to acquire the motion sequences of both hands, obtaining the 3D coordinates of the joint positions of the human arm in a motion sequence. During use, median filtering was applied to smooth the acquired joint data, smoothing out unstable and shaky movements of the arm during the motion to obtain the joint position coordinates of each frame after processing.

[0095] S3. Establish a unified representation of the joint poses of a human arm and the equivalent joint poses of a bi-arm robot. That is, use spherical coordinate representation: Three parameters can uniquely determine a point in space. Since the anthropomorphic joint pose mapping method is completely consistent for both arms, this invention uses a single arm as an example to introduce the specific implementation process of the mapping algorithm, representing the spherical coordinates of the equivalent joint vector of the robotic arm as follows: The joint vectors of the human arm are represented as: Where ρ1 = 360mm, ρ2 = 300mm, ρ3 = 80mm, r1 = 256mm, r2 = 210mm, and r3 = 172.5mm.

[0096] This study verifies whether a dual-arm robot can achieve the same joint postures as a human arm during movement. The comparison results are as follows: Figure 2 As shown in the figure. The verification results show that the range of joint angles achievable by the human arm is within the range of equivalent joint motion achievable by the robotic arm. This result provides a theoretical basis for subsequent anthropomorphic joint pose mapping.

[0097] S4. Represent the joints of the human arm as vectors. The shoulder, elbow, wrist, and hand joints and their coordinates are represented by S. h =(x S y S , z S E h =(x E y E , z E ), W h =(x W y W , z W ), H h =(x H y H , z H The four points represent the upper arm, forearm, and palm vectors, respectively, and the calculated vectors are represented by S. h E h E h W h、 W h H h The following are represented as: S h E h =(x E -x S y E -y S , z E -z S E h W h =(x W -x E y W -y E , z W -z E The equivalent joints of the robotic arm and the joints of the human arm, and their representations, are as follows: Figure 3 and Figure 4 As shown in the diagram. A one-to-one correspondence between the equivalent joints of the robotic arm and the equivalent joints of the human arm is shown below. Figure 6 As shown.

[0098] S h Eh E h W h、 W h H h The coordinate values ​​of the three vectors are converted to spherical coordinates. This allows for the calculation of the equivalent joint vector polar angle θ and azimuth angle without altering them. By changing only the vector length, the spherical coordinate values ​​of the equivalent joint vectors of the robotic arm are obtained. These spherical coordinate values ​​are then converted to three-dimensional Cartesian coordinates, represented as D1 = (x1, y1, z1), D2 = (x2, y2, z2), and D3 = (x3, y3, z3), respectively. The shoulder coordinates of the robotic arm are represented as D0 = (0, 24, 0, 50). The calculated coordinates of the equivalent joints of the target robotic arm—shoulder, elbow, wrist, and hand—are as follows:

[0099]

[0100] The above four coordinates are the target coordinate values ​​of the equivalent joints of the robotic arm. Among them, D0_target is the equivalent shoulder coordinate of the robotic arm, D1_target is the equivalent elbow coordinate of the robotic arm, D2_target is the equivalent wrist coordinate of the robotic arm, and D3_target is the equivalent hand coordinate of the robotic arm.

[0101] S5. Using the particle swarm optimization algorithm requires defining an objective function. This patent expresses the objective function as fitness = norm(e1) + norm(e2) + norm(e3), where the error vector is represented using mean square error. This is achieved by subtracting the actual coordinates of the equivalent joints from the coordinates of the target joints, and summing the mean square errors of the difference vectors as the error vector to be optimized. Specifically:

[0102] e1=P_3-D1_target, e2=P_5-D2_target, e3=P_7-D3_target.

[0103] x i (t+1)=x i (t)+v i (t+1), where x i (t) represents the position of particle i at time t, v i (t+1) represents the velocity of particle i at time t_+1.

[0104] In the particle swarm optimization algorithm, the particle velocity update formula is:

[0105] x i (t+1)=x i (t)+v i (t+1)v i (t+1)=w·v i(t)+c1·r1·(pbest i -x i (t))+c2·r2·(gbest-x i (t))

[0106] Among them, v i (t) is the velocity of particle i at time t; w is the inertia weight; c1 and c2 are learning factors; r1 and t2 are uniformly random numbers between (0,1); pbest i `gbest` represents the known historical best position of the first particle, i.e., the individual optimal value; `gbest` represents the known historical best position of the entire particle swarm, i.e., the global optimal value. After obtaining the new velocity using the velocity update formula described above, the new position of the particle at time `gbest` is calculated using the position update formula. The particle swarm optimization algorithm finds the optimal solution by continuously iterating and updating the velocity and position of the particles.

[0107] The parameter settings for the particle swarm optimization algorithm are as follows:

[0108] n = 100, maxIter = 200, alpha = 0.7, beta = 1.5, gamma = 1.5, where n is the number of particles in the swarm, maxIter is the maximum number of iterations, w is the inertia weight, c1 is the individual learning factor, and c2 is the individual learning factor. The global optimization process is as follows: Figure 7 As shown.

[0109] The calculations ultimately yielded the robotic arm joint angle values ​​that satisfied the objective function, and the error between the obtained robotic arm joint posture and the human arm joint posture was zero.

[0110] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Various modifications and improvements made by those skilled in the art to the technical solutions of the present invention without departing from the spirit of the present invention should fall within the protection scope defined by the claims of the present invention.

Claims

1. A method for anthropomorphic joint pose mapping of a dual-arm robot based on particle swarm optimization algorithm, characterized in that: It includes the following steps: S1. Feasibility analysis of anthropomorphic posture motion of dual-arm robot; S2. Establish the kinematic model of the robotic arm based on the DH parameters of the robotic arm, and use the forward kinematic formula to calculate the position coordinates of the equivalent shoulder, elbow, wrist and palm position coordinate system. S3. Obtain motion data of the joints of both arms from the database, extract the position coordinates of the shoulder, elbow, wrist and palm joints from the motion data of the joints of both arms, and calculate the vector coordinate values ​​of the upper arm, forearm and palm. S4. Using spherical coordinate representation, calculate the spherical coordinates of the vector coordinates of the upper arm, forearm, and hand, i.e., the target spherical coordinate parameter values ​​θ, φ, r of the arm joints; and express the target spherical coordinate parameter values ​​θ, φ, r of the arm joints as the spherical coordinates of the equivalent joints of the robotic arm, thereby obtaining the target spherical coordinate values ​​of the robotic arm. Then, calculate the position coordinates of the target joints of the robotic arm, obtaining the target coordinates of the elbow, wrist, and hand, respectively represented as... ; S5. Use the particle swarm optimization algorithm to obtain the joint angle values ​​of the robotic arm that meet the constraints, calculate the corresponding joint angles frame by frame and save them, obtain the frame-by-frame joint angle values ​​of the robotic arm to complete the entire movement and send them to the robotic arm.

2. The method for anthropomorphic joint pose mapping of a dual-arm robot based on particle swarm optimization algorithm according to claim 1, characterized in that: Step S5 specifically includes the following sub-steps: S51. Construct joint constraint equations: ； in, For the joint angles of the robotic arm, This represents the minimum joint angle of the robotic arm. This represents the maximum joint angle of the robotic arm; S52. Randomly initialize joint angle values ​​and iterate to calculate the transformation matrix from the base coordinate system of the robotic arm to the equivalent joint coordinate system. S53. Calculate the position coordinates of the transformation matrix based on the DH parameters of the robotic arm to obtain the actual coordinates of the equivalent joints of the robotic arm: elbow, wrist, and palm, respectively, expressed as follows: ; S54. By subtracting the actual coordinates of the equivalent joints of the robotic arm from the target coordinates of the elbow, wrist, and palm, and using the mean square error norm of the difference vector as the error vector to be optimized, the fitness function of the particle swarm optimization algorithm is defined as follows: , The error vector is represented using mean square error, specifically: ; Where norm is the Euclidean norm. , , These are the error vectors for the joints of elbow, wrist, and palm, respectively. S55. Filter the motion data of the joints of the two arms, calculate the corresponding joint angles frame by frame using steps S51-S54 for the processed motion sequence and save them, so as to obtain the frame-by-frame joint angle values ​​of the robotic arm to complete the whole motion and send them to the robotic arm.

3. The method for anthropomorphic joint pose mapping of a dual-arm robot based on particle swarm optimization algorithm according to claim 2, characterized in that: In step S52, the transformation matrices from the base coordinate system of the robotic arm to the equivalent joint coordinate system are as follows: ； ； ； ； ； in, This refers to the joint offset of the robotic arm. For joint angle, For the connecting rod torsion angle, Let be the transformation matrix from the (i-1)th coordinate system of the robotic arm to the ith coordinate system. Let be the transformation matrix for the robotic arm from the base coordinate system to the third coordinate system. Let be the transformation matrix for the robotic arm from the base coordinate system to the fifth coordinate system. This is the transformation matrix for the robotic arm from the base coordinate system to the n-coordinate system.

4. The method for anthropomorphic joint pose mapping of a dual-arm robot based on particle swarm optimization algorithm according to claim 3, characterized in that: In step S54, the position update formula for the particle swarm optimization algorithm is: ,in, Represents particles exist Location at any given moment Represents particles exist The velocity at time _+1; In the particle swarm optimization algorithm, the particle velocity update formula is: ； in, It is a particle exist The speed of time; Inertial weight; and It is a learning factor; and yes A uniformly random number between [a certain value]; The known historical best position of the first particle, i.e., the individual best value; To find the global optimal value, after obtaining the new velocity using the above velocity update formula, the new position of the particle at that moment is calculated using the position update formula. The optimal solution is found by iteratively updating the particle's velocity and position.

5. The method for anthropomorphic joint pose mapping of a dual-arm robot based on particle swarm optimization algorithm according to claim 1, characterized in that: Step S1 is as follows: Based on the spherical coordinate representation of vectors θ, φ, r, within the joint constraints of the robotic arm, the range of spherical coordinate parameters of the equivalent joints of the robotic arm is obtained through the robot's forward kinematics model. Within the joint constraints of the human arm, the range of spherical coordinate parameters of the shoulder, elbow, wrist, and hand joints is solved. The magnitudes of the two are compared, and it is found that the range of parameter values ​​of the equivalent joints of the robotic arm is greater than the range of spherical coordinate parameters of the corresponding joints of the human arm.

6. The method for anthropomorphic joint pose mapping of a dual-arm robot based on particle swarm optimization algorithm according to claim 1, characterized in that: Step S2 is as follows: Given the DH parameters of the robotic arm, construct the kinematic model of the dual-arm robot in MATLAB, use forward kinematics to calculate the transformation matrix of the equivalent shoulder, elbow, wrist and hand positions, extract the displacement vector of the transformation matrix, which is the position coordinate of the equivalent joint of the robotic arm, and the joint angle vector of the robotic arm corresponding to the coordinate is the position coordinate of the equivalent shoulder, elbow, wrist and hand position coordinate system.

7. The method for anthropomorphic joint pose mapping of a dual-arm robot based on particle swarm optimization algorithm according to claim 1, characterized in that: Step S3 specifically involves: using the coordinates of the shoulder, elbow, wrist, and hand joints... The four points represent the calculated vector coordinates of the upper arm, forearm, and palm, respectively. , E h W h , .

8. The method for anthropomorphic joint pose mapping of a dual-arm robot based on particle swarm optimization algorithm according to claim 1, characterized in that: Step S4 specifically includes the following sub-steps: S41. Vector coordinates of upper arm, forearm, and hand , , The spherical coordinates are represented as , , ; S42. Based on the spherical coordinate representation of the vector coordinates of the upper arm, forearm, and hand, the target spherical coordinate parameter values ​​of the arm joints are expressed as the spherical coordinates of the equivalent joints of the robotic arm. Then, the target joint vector position coordinates of the robotic arm are calculated, and the spherical coordinates of the equivalent joints of the robotic arm are respectively... , , ; S43. Concatenate the three vectors of the target joint position coordinates of the robotic arm in the order of upper arm, forearm and palm to obtain the target coordinates of the robotic arm elbow, wrist and palm.

9. The method for anthropomorphic joint pose mapping of a dual-arm robot based on particle swarm optimization algorithm according to claim 1, characterized in that: The dual-arm robot is a seven-degree-of-freedom articulated robot. The database in step S3 is obtained by collecting motion data of the joints of the two arms using the NOKOV metric optical 3D motion capture device and storing it in real time.

10. The method for anthropomorphic joint pose mapping of a dual-arm robot based on particle swarm optimization algorithm according to claim 2, characterized in that: In step S51, the rotation range of joint 1 is ±178°, the rotation range of joint 2 is ±130°, the rotation range of joint 3 is ±178°, the rotation range of joint 4 is ±135°, the rotation range of joint 5 is ±178°, the rotation range of joint 6 is ±128°, and the rotation range of joint 7 is ±360°.

Images