Two-arm robot anthropomorphic joint attitude mapping method based on particle swarm optimization algorithm

Abstract

The invention relates to a two-arm robot anthropomorphic joint attitude mapping method based on a particle swarm optimization algorithm, belongs to the field of robot kinematics and trajectory planning, and mainly comprises the step of establishing a unified representation method, namely a spherical coordinate representation method, of a human two-arm joint pose and a robot two-arm joint pose. A large arm, a small arm and a palm joint of a person are equivalent to three vectors which are connected in sequence, a mechanical arm is equivalent to the large arm, the small arm and the palm joint, each joint is regarded as a vector, and the three vectors are connected in sequence; according to the method, spatial positions of a human arm three-joint vector and a mechanical arm three-joint vector are determined by using a spherical coordinate representation method, and polar coordinates and azimuth angle parameters of an equivalent joint vector of the double-arm robot and the human arm joint vector are enabled to be equal through an anthropomorphic joint attitude mapping algorithm; and then the mechanical arm joint angle value meeting the condition is obtained through a particle swarm optimization algorithm, and therefore migration from the human arm joint posture to the humanoid posture of the double-arm robot is achieved.

Description

Technical Field

[0001] The invention belongs to the field of robot kinematics and trajectory planning, and in particular relates to a dual-arm robot anthropomorphic joint posture mapping method based on a particle swarm optimization algorithm. Background Art

[0002] With the development of robot motion control and trajectory planning technology, humans have endowed dual-arm robots with a variety of functions. To achieve dual-arm robots to collaborate with humans or imitate human operations efficiently, accurately and naturally, a key technical challenge is how to achieve anthropomorphic joint posture mapping of dual-arm robots. In many fields such as modern manufacturing, logistics, medical rehabilitation, and services, dual-arm robots are increasingly widely used, and they show great potential in improving production efficiency, enhancing operational flexibility, and achieving human-machine collaboration. Allowing dual-arm robots to have the same motion posture as human hands in the process of performing tasks can significantly increase the affinity of dual-arm robots and the interpretability of motion, while making dual-arm robots easier to enter people's lives. To be more precise, the posture of the robot's arms is the most important traffic signal when the traffic control robot performs traffic dredging tasks. Therefore, it is particularly important for the robot to accurately imitate the arm posture of the traffic police when performing tasks.

[0003] In recent years, a large number of deep learning-based anthropomorphic joint posture mapping for dual-arm robots have emerged. First of all, this method requires researchers to have a strong background in computer and artificial intelligence. When using deep learning to implement robot joint posture mapping, a large amount of training data is needed to learn effective feature representation 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 explain. The training and reasoning processes of deep learning models usually require a large amount of computing resources, including high-performance graphics processing units.

[0004] Secondly, the use of reinforcement learning algorithms usually requires robots to conduct a lot of trial and error exploration in the environment to learn the optimal strategy. This trial and error process is often very time-consuming in practical applications. In the process of searching for the optimal strategy, reinforcement learning algorithms are prone to fall into local optimal solutions, especially in complex high-dimensional action spaces (such as the joint posture space of a dual-arm robot), where there are many local optimal solutions. Therefore, a new mapping algorithm is urgently needed to solve the above problems. Summary of the invention

[0005] In view of the shortcomings of the prior art, the present invention provides a dual-arm robot anthropomorphic joint posture mapping method based on a particle swarm optimization algorithm. The method equates the upper arm, forearm and palm joints of a person to three sequentially connected vectors, and equates the robotic arm to the upper arm, forearm and palm joints. Each joint is regarded as a vector, and the three vectors are connected in sequence. Then, the spatial positions of the three-joint vectors of the human arm and the three-joint vectors of the robotic arm are determined by using a spherical coordinate representation method. The anthropomorphic joint posture mapping algorithm is used to make the polar coordinates and azimuth parameters of the equivalent joint vector of the dual-arm robot equal to those of the human arm joint vector. Then, the particle swarm optimization algorithm is used to obtain the robotic arm joint angle values ​​that meet the conditions, thereby realizing the migration from the human arm joint posture to the dual-arm robot anthropomorphic posture.

[0006] To achieve the above object, the present invention provides the following technical solutions:

[0007] The present invention provides a dual-arm robot anthropomorphic joint posture mapping method based on a particle swarm optimization algorithm, which comprises the following steps:

[0008] S1. Analyze the feasibility of the anthropomorphic posture movement of the dual-arm robot;

[0009] S2. Establish a kinematic model of the robot arm according to the DH parameters of the robot arm, and use the forward kinematics formula to calculate the position coordinates of the equivalent shoulder, elbow, wrist and palm position coordinate systems;

[0010] S3, obtaining the motion data of the joints of both arms from the database, extracting the position coordinates of the joints of the shoulder, elbow, wrist and palm from the motion data of the joints of both arms, and calculating the vector coordinate values ​​of the upper arm, forearm and palm;

[0011] S4. Use the spherical coordinate representation method to calculate the spherical coordinate representation of the vector coordinates of the upper arm, forearm and palm, that is, the target spherical coordinate parameter values ​​(θ, φ, r) of the arm joint points; and express the target spherical coordinate parameter values ​​(θ, φ, r) of the arm joint points 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 joint points of the robotic arm to obtain the target coordinates of the robotic arm elbow, wrist and palm, which are expressed 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 transmit 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] Among them, qi is the joint angle of the robot arm, qi lower is the minimum value of the robot arm joint angle, qi upper is the maximum value of the robot arm joint angle;

[0017] S52, randomly initialize the joint angle value and iterate, and calculate the transformation matrix from the base coordinate system of the robot arm to the equivalent joint point coordinate system;

[0018] S53, calculating the position coordinates of the transformation matrix according to the DH parameters of the robot arm, and obtaining the actual coordinates of the equivalent joint points of the robot arm, 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 joint points of the robotic arm from the target coordinates of the robotic arm elbow, wrist and palm, the mean square error norm of the difference vector is used as the error vector to be optimized, and the fitness function of the particle swarm optimization algorithm is defined as fitness = norm(e1) + norm(e2) + norm(e3),

[0020] Among them, the error vector is expressed by mean square error, specifically:

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

[0022] Among them, norm is the Euclidean norm, e1, e2, and e3 are the error vectors of the joint points elbow, wrist, and palm respectively;

[0023] S55. Filter the motion data of the joint points of both arms, calculate the corresponding joint angles frame by frame of the processed action sequence using steps S51-S54 and save them, obtain the frame-by-frame joint angle values ​​of the robotic arm completing the entire movement and transmit them to the robotic arm.

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

[0025]

[0026] 0 T n = 0 T 1 × 1 T 2 × 2 T 3 ×…×n -1 Tn

[0027] 0 T 3 = 0 T 1 × 1 T 2 × 2 T 3

[0028] 0 T 5 = 0 T 1 × 1 T 2 × 2 T 3 × 3 T 4 × 4 T 5

[0029] 0 T 7 = 0 T 1 × 1 T 2 × 2 T 3 × 3 T 4 × 4 T 5 × 5 T 6 × 6 T 7 ;

[0030] Among them, d i is the joint offset of the robot arm, θ i is the joint angle, α i-1 is the connecting rod torsion angle, i-1 T i is the transformation matrix from the i-1th coordinate system of the robot to the i-th coordinate system, 0 T 3 is the transformation matrix of the robot from the base coordinate system to the third coordinate system, 0 T 5 is the transformation matrix of the robot from the base coordinate system to the fifth coordinate system, 0 T n is the transformation matrix of the robot 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)+vi (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)+c 1 ·r 1 ·(pbest i -x i (t))+c 2 ·r 2 ·(gbest-x i (t))

[0035] Among them, v i (t) is the velocity of particle i at time t; w is the inertia weight; c 1 and c 2 is the learning factor; r 1 and r 2 is a uniform random number between (0,1); pbest i is the best known historical position of the first particle, that is, the individual optimal value; gbest is the global optimal value. After obtaining the new speed through the above speed update formula, the position update formula is used to calculate the new position of the particle at this moment, and the optimal solution is found by continuously iteratively updating the speed and position of the particle.

[0036] Preferably, step S1 is specifically as follows: according to the spherical coordinate representation of the vector (θ, φ, r), within the joint restriction range of the robotic arm, the spherical coordinate parameter range of the equivalent joint points of the robotic arm is obtained through the forward kinematics model of the robot, and the spherical coordinate parameter range of the shoulder, elbow, wrist and palm joint points of the human arm is solved within the joint restriction range, and the sizes of the two are compared to obtain the parameter value range of the equivalent joint points of the robotic arm that is greater than the spherical coordinate parameter range of the corresponding human arm joint points.

[0037] Preferably, step S2 is specifically as follows: given the DH parameters of the robotic arm, a kinematic model of a dual-arm robot is constructed in mat lab, forward kinematics is used to calculate the transformation matrix of the equivalent shoulder, elbow, wrist and palm positions, and the displacement vector of the transformation matrix is ​​extracted, which is the position coordinate of the equivalent joint point of the robotic arm. The joint angle vector of the robotic arm corresponding to the coordinate is the position coordinate of the equivalent shoulder, elbow, wrist and palm position coordinate system.

[0038] Preferably, step S3 specifically comprises: using S to calculate the coordinates of the shoulder, elbow, wrist and palm joints of the person. h 、E h , W h , H h The four points represent that the calculated vector coordinates of the upper arm, forearm and palm are S h E h , W h H h , W h H h .

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

[0040] S41, the upper arm, forearm and palm vector coordinates S h E h , E h W h , W h H h The spherical coordinates of

[0041] S42, according to the spherical coordinate representation of the upper arm, forearm and palm vector coordinates, the target spherical coordinate parameter value of the arm joint point is represented as the spherical coordinate of the equivalent joint of the robotic arm, and then the target joint vector position coordinate of the robotic arm is calculated. The equivalent joint spherical coordinates of the robotic arm are respectively expressed as

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

[0043] Preferably, the dual-arm robot is a seven-degree-of-freedom articulated robot, and the database in step S3 uses a NOKOV metric optical three-dimensional motion capture device to collect and store motion data of dual-arm joints.

[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) The present invention uses a vector method to represent the joint postures of a robot arm and a human arm, and utilizes the properties of vectors to make the equivalent joint postures of the robot arm and the joint postures of the human arm equivalent to the end-to-end connection of multiple vector segments. At the same time, the spherical coordinate method is used to unify the representation of the equivalent joint vectors of the robot arm and the joint vectors of the human arm, thereby building a bridge between the two representation methods. Specifically, the present invention converts the joint point coordinates of the human arm into joint vectors, and then converts them into a spherical coordinate representation. The spherical coordinates of the joint vectors of the human arm are equivalent to the spherical coordinates of the equivalent joint vectors of the robot arm, and the spherical coordinates of the equivalent joint vectors of the robot arm are converted into rectangular coordinate representation. The rectangular coordinates of the equivalent joint vectors of the robot arm are used as the target position of the anthropomorphic joint posture of the robot arm, thereby realizing the migration from the joint posture of the human arm to the anthropomorphic posture of the dual-arm robot. The entire process has a small amount of calculation and can ensure the accuracy of the anthropomorphic posture of the dual-arm robot.

[0047] (2) The method of the present invention uses an optical motion capture device to capture the movements of human arm joints, and stores the position coordinates of the shoulder, elbow, wrist and palm joints of the arm in a database, then obtains the motion data of both arm joints from the database, extracts the position coordinates of the shoulder, elbow, wrist and palm joints from the motion data of both arm joints, and calculates the vector coordinates of the upper arm, forearm and palm, so as to make the anthropomorphic posture of the dual-arm robot more consistent with the movements of the human arm joints.

[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 under the joint angle at the previous moment as the objective function, and using the particle swarm optimization algorithm to gradually solve a set of joint angle values ​​when the objective function takes the minimum value. The calculation amount is small and the optimal solution can be obtained quickly. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 A specific flow chart for the implementation of the present invention;

[0050] Figure 2 A comparison diagram of the spherical coordinate parameters of the equivalent joints of the robot arm of the present invention and the spherical coordinate parameters of the joint vectors of the human arm;

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

[0052] Figure 4 A schematic diagram of the joint points and joint vectors of a human arm of the present invention;

[0053] Figure 5 This is a schematic diagram of capturing the motion of a human arm joint using the optical motion capture device of the present invention;

[0054] Figure 6 A schematic diagram showing the comparison between the joint vectors of a human arm and the equivalent joint vectors of a robotic arm of the present invention;

[0055] Figure 7 A schematic diagram of the process of finding the global optimum by the particle swarm optimization algorithm of the present invention. DETAILED DESCRIPTION

[0056] The exemplary embodiments, features and aspects of the present invention will be described in detail below with reference to the accompanying drawings. The same reference numerals in the accompanying drawings represent elements with the same or similar functions. Although various aspects of the embodiments are shown in the accompanying drawings, the drawings are not necessarily drawn to scale unless otherwise specified.

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

[0058] S1. Analyze the feasibility of the anthropomorphic posture movement of the dual-arm robot. The structural characteristics and size of the dual-arm robot's mechanical arm are quite different from the joint size and movement mechanism of the human arm. Therefore, whether the mechanical arm can imitate the joint posture characteristics of the human arm needs to be verified in advance. If the mechanical arm can imitate the joint posture of the human arm, then a feasible range of motion is given. If it is feasible within a partial range of motion, then the same posture is solved in the accessible space, and the joint angle is constrained within the unreachable range, or the topological joint posture is solved. Therefore, it is necessary to first analyze the feasibility of the anthropomorphic posture movement of the dual-arm robot and verify whether the spherical coordinate parameter range of the equivalent joint vector of the dual-arm robot is greater than the spherical coordinate parameter range of the upper arm, forearm and palm vectors of the human arms.

[0059] S2. Establish the kinematic model of the robot arm according to the DH parameters of the robot arm. Use the forward kinematics formula to calculate the position coordinates of the equivalent shoulder, elbow, wrist and palm position coordinate system. The specific process is: given the DH parameters of the robot arm, build 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 palm position, extract the displacement vector of the transformation matrix, which is the position coordinate of the equivalent joint point of the robot arm, and the joint angle vector of the robot arm corresponding to the coordinate is the position coordinate of the equivalent shoulder, elbow, wrist and palm position coordinate system.

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

[0061] S4. Use the spherical coordinate representation method to calculate the spherical coordinate representation (θ, φ, r) of the vector coordinates of the upper arm, forearm and palm; express the target spherical coordinate parameter value of the arm joint point as the spherical coordinate of the equivalent joint of the robot arm, and then obtain the target spherical coordinate value of the robot arm, calculate the position coordinates of the target joint point of the robot arm, and obtain the target posture of the robot arm. The spherical coordinates can uniquely determine the position of a point in space through three parameters, the direction and size of the equivalent joint vector of the robot arm can be determined by the spherical coordinate method, and the direction and size of the human arm joint can also be determined by the spherical coordinate method; express the target spherical coordinate parameter value of the human arm joint point as the spherical coordinate parameter value of the equivalent joint of the robot arm, and then obtain the target spherical coordinate value of the robot arm, and then calculate the position coordinates of the target joint point of the robot arm.

[0062] Specifically, the shoulder, elbow, wrist and palm joints and coordinates of the human body are expressed as 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 ) four points, the joint point coordinates are obtained by NOKOV metric optical 3D motion capture device and stored in the database. After extraction, the upper arm, forearm and palm vectors are calculated and expressed as S h E h 、E h W h、 W h H h Indicates, 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 spherical coordinates of the three vectors are expressed as The joint lengths of the arms are ρ1=360mm, ρ2=300mm, and ρ3=80mm.

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

[0068] The spherical coordinate parameters of the human arm joint vector are matched with the spherical coordinate parameters of the equivalent joint of the robot arm. Since the size and structure of the robot arm are quite different from the joint length and connection mode of the human arm, the length of the equivalent joint is different in the equivalent process, so the length of the equivalent joint vector is excluded in the process of using spherical coordinates. Thus, the target coordinates of the joint vector of the robot arm are obtained.

[0069] The spherical coordinate representation of the equivalent joint vector of the robot arm is converted into a rectangular coordinate representation. The shoulder, elbow, wrist and palm are represented as D0, D1, D2, and D3 respectively. The equivalent shoulder coordinate of the robot arm is represented as D0 = [0240.50]. The coordinates of the elbow, wrist and palm positions of the equivalent joint points of the target robot arm are calculated 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 joint points of the robot arm. Among them, D0_target is the equivalent shoulder coordinate of the robot arm, D1_target is the equivalent elbow coordinate of the robot arm, D2_target is the equivalent wrist coordinate of the robot arm, and D3_target is the equivalent palm coordinate of the robot arm.

[0073] S5. Use the particle swarm optimization algorithm to obtain the robot arm joint angle value that meets the constraint conditions. The specific steps are as follows:

[0074] S51, the joint constraint equation is: qi lower ≤qi≤qi upper In this embodiment, the dual-arm robot is a seven-degree-of-freedom joint 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 robot arm to the equivalent joint point coordinate system is calculated, which are:

[0076]

[0077] 0 T n = 0 T 1 × 1 T 2 × 2 T 3 ×…×n -1 T n

[0078] 0 T 3 = 0 T 1 × 1 T 2 × 2 T 3

[0079] 0 T 5 = 0 T 1 ×1 T 2 × 2 T 3 × 3 T 4 × 4 T 5

[0080] 0 T 7 = 0 T 1 × 1 T 2 × 2 T 3 × 3 T 4 × 4 T 5 × 5 T 6 × 6 T 7 .

[0081] Among them, d i is the joint offset of the robot arm, θ i is the joint angle, α i-1 is the connecting rod torsion angle, i-1 T i is the transformation matrix from the i-1th coordinate system of the robot to the i-th coordinate system, 0 T 3 is the transformation matrix of the robot from the base coordinate system to the third coordinate system, 0 T 5 is the transformation matrix of the robot from the base coordinate system to the fifth coordinate system, 0 T n is the transformation matrix of the robot from the base coordinate system to the n coordinate system.

[0082] S53, calculate the position coordinates of the transformation matrix according to the DH parameters of the robot arm, and obtain the actual coordinates of the equivalent joint points of the robot arm, shoulder, elbow, and wrist, which are expressed as P_3, P_5, and P_7 respectively; in this embodiment, the actual coordinates of the equivalent joint points are: 0 T 3 , 0 T 5 , The first three rows of the last column are P_3=S_3.t, P_5=S_5.t, P_7=S_7.t.

[0083] S54, by subtracting the actual coordinates of the equivalent joint points from the target posture of the robot shoulder, elbow and wrist, and taking the mean square error norm of the difference vector as the error vector to be optimized, first define the fitness function of the particle swarm optimization algorithm 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 of 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)+c 1 ·r 1 ·(pbest i -x i (t))+c 2 ·r 2 ·(gbest-x i (t))

[0089] Among them, v i (t) is the velocity of particle i at time t; w is the inertia weight; c 1 and c 2 is the learning factor; r 1 and t 2 is a uniform random number between (0,1); pbest i is the best known historical position of the first particle, i.e., the individual optimal value; gbest is the best known historical position of the entire particle swarm, i.e., the global optimal value. After obtaining the new speed through the above speed update formula, the position update formula is used to calculate the new position of the particle at time. The particle swarm algorithm is to find the optimal solution by continuously iteratively updating the speed and position of the particles.

[0090] S55. The joint point motion data of the human arm captured by the motion capture device is smoothed and filtered using a median filter to smooth the unstable and shaking movements of the arm during the movement; the corresponding joint angles are calculated frame by frame for the processed action sequence, and then saved, and finally the frame-by-frame joint angle values ​​of the robotic arm completing the entire movement are obtained. The obtained joint angle values ​​are sent to the robotic arm, and the robotic arm can accurately perform the movements demonstrated by the experimenter. The joint posture similarity of the robotic arm movement is evaluated using the minimum value of the fitness function. After being solved by the particle swarm optimization algorithm, the minimum error of the fitness function is 0. Therefore, it can be known that the use of this human-machine joint posture mapping algorithm can obtain a very good mapping effect. Specific embodiments

[0092] The present invention provides a dual-arm robot anthropomorphic joint posture mapping method based on particle swarm optimization algorithm, such as Figure 1 As shown, the specific steps are:

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

[0094] S2. Optical marker balls are attached to the shoulders, elbows, wrists and palms of both arms. The capture effect is as follows: Figure 5 As shown. The NOKOV metric optical 3D motion capture device is used to collect the motion sequence of both hands, and the 3D coordinate values ​​of the position of the joint points of the human arm in a motion sequence are obtained. When in use, the collected joint point data is smoothed and filtered by using a median filter, and the unstable and shaking movements of the arms during the movement are smoothed to obtain the joint point position coordinates of each frame after processing.

[0095] S3. Establish a unified representation of the joint posture of human arms and the equivalent joint posture of dual-arm robots. That is, use spherical coordinate representation: The three parameters can uniquely determine a point in space. Since the anthropomorphic joint posture mapping method of both arms is completely consistent, this paper takes a single arm as an example to introduce the specific implementation process of the mapping algorithm, and expresses the spherical coordinates of the equivalent joint vector of the robotic arm as The joint vector of the human arm is represented as: Among them, ρ1=360mm, ρ2=300mm, ρ3=80mm, r1=256mm, r2=210mm, r3=172.5mm.

[0096] Verify whether the dual-arm robot can achieve the same joint posture as a human arm during movement. The comparison results between the two are as follows: Figure 2The verification results show that the range of joint angles of the human arm is within the range of the equivalent joint motion of the robotic arm. This result provides a theoretical basis for the subsequent anthropomorphic joint posture mapping.

[0097] S4. Represent the joints of the human arm as vectors. The joint points and coordinates of the human shoulder, elbow, wrist and palm 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 ) four points, the calculated upper arm, lower arm and palm vectors are represented by S h E h 、E h W h、 W h H h Indicates, respectively: 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 positions and representations of the equivalent joint points of the robot arm and the joint points of the human arm are as follows Figure 3 and Figure 4 The one-to-one correspondence between the equivalent joint points of the robot arm and the equivalent joint points of the human arm is shown in Figure 6 shown.

[0098] S h E h 、E h W h、 W h H h The coordinate values ​​of the three vectors are converted to spherical coordinates. Then, without changing the equivalent joint vector polar angle θ and azimuth angle When only the vector length is changed, the spherical coordinates of the equivalent joint vector of the manipulator are obtained. The spherical coordinates of the equivalent joint vector are converted into three-dimensional rectangular coordinates, respectively expressed as D1 = (x1, y1, z1), D2 = (x2, y2, z2), D3 = (x3, y3, z3), where the shoulder coordinates of the manipulator are expressed as D0 = (0240.50), and the coordinates of the shoulder, elbow, wrist and palm positions of the equivalent joint points of the target manipulator are calculated as follows:

[0099]

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

[0101] S5. The particle swarm optimization algorithm needs to define the objective function to be optimized. This patent expresses the objective function as fitness = norm(e1) + norm(e2) + norm(e3), where the error vector is represented by the mean square error. By subtracting the actual coordinates of the equivalent joint point from the coordinates of the target joint point, the sum of the mean square errors of the difference vectors is taken 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)+c 1 ·r 1 ·(pbest i -x i (t))+c 2 ·r 2 ·(gbest-xi (t))

[0106] Among them, v i (t) is the velocity of particle i at time t; w is the inertia weight; c 1 and c 2 is the learning factor; r 1 and t 2 is a uniform random number between (0,1); pbest i is the best known historical position of the first particle, i.e., the individual optimal value; gbest is the best known historical position of the entire particle swarm, i.e., the global optimal value. After obtaining the new speed through the above speed update formula, the position update formula is used to calculate the new position of the particle at time. The particle swarm algorithm is to find the optimal solution by continuously iteratively updating the speed and position of the particles.

[0107] The parameter settings of 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 individuals in the particle swarm, maxIter is the maximum number of iterations, w is the inertia weight, c1 is the individual learning factor, c2 is the individual learning factor, and the global optimization process is as follows: Figure 7 shown.

[0109] After calculation, the robot arm joint angle value that meets the objective function is finally obtained, and the error between the obtained robot arm joint posture and the human arm joint posture is zero.

[0110] The embodiments described above are only descriptions of the preferred implementation modes of the present invention, and are not intended to limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by ordinary technicians in this field should all fall within the protection scope determined by the claims of the present invention.

Claims

1. A dual-arm robot anthropomorphic joint posture mapping method based on particle swarm optimization algorithm, characterized by: It includes the following steps: S1. Analyze the feasibility of the anthropomorphic posture movement of the dual-arm robot; S2. Establish a kinematic model of the robot arm according to the DH parameters of the robot arm, and use the forward kinematics formula to calculate the position coordinates of the equivalent shoulder, elbow, wrist and palm position coordinate systems; S3, obtaining the motion data of the joints of both arms from the database, extracting the position coordinates of the joints of the shoulder, elbow, wrist and palm from the motion data of the joints of both arms, and calculating the vector coordinate values ​​of the upper arm, forearm and palm; S4. Use the spherical coordinate representation method to calculate the spherical coordinate representation of the vector coordinates of the upper arm, forearm and palm, that is, the target spherical coordinate parameter values ​​θ, φ, r of the arm joint points; and express the target spherical coordinate parameter values ​​θ, φ, r of the arm joint points 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 joint points of the robotic arm to obtain the target coordinates of the elbow, wrist and palm of the robotic arm, which are expressed as D1_target, D2_target, and D3_target, respectively; 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 transmit them to the robotic arm.

2. The method for anthropomorphic joint posture mapping of a dual-arm robot based on particle swarm optimization algorithm according to claim 1 is characterized in that: Step S5 specifically includes the following sub-steps: S51. Construct joint constraint equations: qi lower ≤qi≤qi upper ; Among them, qi is the joint angle of the robot arm, qi lower is the minimum value of the robot arm joint angle, qi upper is the maximum value of the robot arm joint angle; S52, randomly initialize the joint angle value and iterate, and calculate the transformation matrix from the base coordinate system of the robot arm to the equivalent joint point coordinate system; S53, calculating the position coordinates of the transformation matrix according to the DH parameters of the robot arm, and obtaining the actual coordinates of the equivalent joint points of the robot arm, elbow, wrist and palm, which are represented as P_3, P_5 and P_7 respectively; S54, by subtracting the actual coordinates of the equivalent joint points of the robot arm from the target coordinates of the elbow, wrist and palm of the robot arm, the mean square error norm of the difference vector is used as the error vector to be optimized, and the fitness function of the particle swarm optimization algorithm is defined as fitness = norm(e1) + norm(e2) + norm(e3), Among them, the error vector is expressed by mean square error, specifically: e1=P_3-D1_target, e2=P_5-D2_target, e3=P_7-D3_target; Among them, norm is the Euclidean norm, e1, e2, and e3 are the error vectors of the joint points elbow, wrist, and palm respectively; S55. Filter the motion data of the joint points of both arms, calculate the corresponding joint angles frame by frame of the processed action sequence using steps S51-S54 and save them, obtain the frame-by-frame joint angle values ​​of the robotic arm completing the entire movement and transmit them to the robotic arm.

3. The method for anthropomorphic joint posture mapping of a dual-arm robot based on particle swarm optimization algorithm according to claim 2 is characterized in that: In step S52, the transformation matrices from the base coordinate system of the robot arm to the equivalent joint point coordinate system are: 0 T n = 0 T1× 1 T2× 2 T3×…× n-1 T n <h2 style=";text-align:left;direction:ltr"> 0 <h2 style=";text-align:left;direction:ltr"> T3=<h2 style=";text-align:left;direction:ltr"> 0 <h2 style=";text-align:left;direction:ltr"> T1×<h2 style=";text-align:left;direction:ltr"> 1 <h2 style=";text-align:left;direction:ltr"> T2×<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> T3 0 T5= 0 T1× 1 T2× 2 T3× 3 T4× 4 T5 <h2 style=";text-align:left;direction:ltr"> 0 <h2 style=";text-align:left;direction:ltr"> T7=<h2 style=";text-align:left;direction:ltr"> 0 <h2 style=";text-align:left;direction:ltr"> T1×<h2 style=";text-align:left;direction:ltr"> 1 <h2 style=";text-align:left;direction:ltr"> T2×<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> T3×<h2 style=";text-align:left;direction:ltr"> 3 <h2 style=";text-align:left;direction:ltr"> T4×<h2 style=";text-align:left;direction:ltr"> 4 <h2 style=";text-align:left;direction:ltr"> T5×<h2 style=";text-align:left;direction:ltr"> 5 <h2 style=";text-align:left;direction:ltr"> T6×<h2 style=";text-align:left;direction:ltr"> 6 <h2 style=";text-align:left;direction:ltr"> T7; Among them, d i is the joint offset of the robot arm, θ i is the joint angle, α i-1 is the connecting rod torsion angle, i-1 T i is the transformation matrix from the i-1th coordinate system of the robot to the i-th coordinate system, 0 T3 is the transformation matrix of the robot from the base coordinate system to the third coordinate system. 0 T5 is the transformation matrix of the robot from the base coordinate system to the fifth coordinate system. 0 T n is the transformation matrix of the robot from the base coordinate system to the n coordinate system.

4. The method for mapping anthropomorphic joint postures of a dual-arm robot based on particle swarm optimization algorithm according to claim 3 is characterized in that: In step S54, the position update formula of the particle swarm optimization algorithm is: 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; In the particle swarm optimization algorithm, the particle velocity update formula is: 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)) 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 uniform random numbers between (0,1); pbest i is the best known historical position of the first particle, that is, the individual optimal value; gbest is the global optimal value. After obtaining the new speed through the above speed update formula, the position update formula is used to calculate the new position of the particle at this moment, and the optimal solution is found by continuously iteratively updating the particle's speed and position.

5. The method for anthropomorphic joint posture mapping of a dual-arm robot based on particle swarm optimization algorithm according to claim 1, characterized in that: Step S1 is specifically as follows: according to the spherical coordinate representation of the vector θ, φ, r, within the joint limit range of the robotic arm, the spherical coordinate parameter range of the equivalent joint points of the robotic arm is obtained through the forward kinematics model of the robot, and the spherical coordinate parameter range of the shoulder, elbow, wrist and palm joint points of the human arm is solved within the joint limit range, and the sizes of the two are compared to obtain that the parameter value range of the equivalent joint points of the robotic arm is greater than the spherical coordinate parameter range of the corresponding human arm joint points.

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

7. The method for mapping anthropomorphic joint postures of a dual-arm robot based on particle swarm optimization algorithm according to claim 1, characterized in that: Step S3 is as follows: the coordinates of the shoulder, elbow, wrist and palm joints of the person are converted into h 、E h , W h , H h The four points represent that the calculated vector coordinates of the upper arm, forearm and palm are S h E h , W h H h , W h H h .

8. The method for mapping anthropomorphic joint postures 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, the upper arm, forearm and palm vector coordinates S h E h , E h W h , W h H h The spherical coordinates of S42, according to the spherical coordinate representation of the upper arm, forearm and palm vector coordinates, the target spherical coordinate parameter value of the arm joint point is represented as the spherical coordinate of the equivalent joint of the robotic arm, and then the target joint vector position coordinate of the robotic arm is calculated. The equivalent joint spherical coordinates of the robotic arm are respectively S43. Concatenate the three vectors of the target joint vector position coordinates of the robotic arm in the order of the upper arm, the lower arm and the palm to obtain the target coordinates of the robotic arm elbow, wrist and palm.

9. The method for mapping anthropomorphic joint postures 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 joint robot. The database in step S3 uses a NOKOV metric optical three-dimensional motion capture device to collect motion data of dual-arm joint points and stores them in real time.

10. The method for mapping anthropomorphic joint postures of a dual-arm robot based on particle swarm optimization algorithm according to claim 8, 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