Mapping method from human to robot arms with different kinematics

Abstract

The accurate modeling from human arms to humanoid machine arms is the premise of humanoid robot's anthropomorphic compliant movement. In this paper, a mapping method simulating human upper limb motion mechanism is proposed to solve this problem. This method reconstructs the human arm model quickly and maps it to the dual-arm robot model. Firstly, the movement mechanism of human upper limb is analyzed and the upper limb model is established. According to the human double-arm kinematics theory, the calculation of arm motion rotation center and the analysis of force and torque are carried out. Then the mapping concept is used to start from the human morphological structure, establish a robot model similar to the arm structure and motion of the human hand, and generate the anthropomorphic configuration. At last, the data collected from the movement of human double arms are matched with the robot model, and the experiment verifies the feasibility of the proposed mapping method.

Introduction

At present, with the development of dual-arm robot research, the structure and function of human dual arm have been applied to the dual arm of robot, which greatly improves the flexibility and application range of dual arm. However, the human arm movement is often quite complex, and its movement is affected by the environment, human heart, physiology, and the use of tools (machinery). As one of the research fields in bionics, the research of humanoid robot needs human beings to understand their own biological structure and motion characteristics in essence to make greater breakthroughs. The humanoid double-arm robot developed is more anthropomorphic in performance, more realistic, and flexible in practical work and has more advantages than single arm in complex and changing environments.

In the upper limb movement, active shoulder joint motion simulation can be said to provide a better understanding of biomechanics. Giles et al. ¹ developed a shoulder joint motion simulation simulator using real-time motion feedback and closed-loop PID control to evaluate the motion performance of human shoulder joints. In order to improve the motion quality of the arm, it is necessary to study the coordinated motion of the joint. Sternad et al. ² the variable conditions on which coordination depends have been analyzed and studied in detail, the measurement index of coordinated movement determines the quality of movement.^3–5 Chen et al. ⁶ the movement measurement of unilateral and bilateral arm movements was studied, and the results were applied to the rehabilitation therapy of perceiving arm daily activities. Literature ⁷ presents a computational framework for generating reaching movement endowed with human motion characteristics that imitated the mechanism in the control and realization of human upper limb motions. Literature ⁸ proposes a hybrid mapping method to solve mapping grasps from human to anthropomorphic robotic hands problem. The proposed hybrid mapping method can not only generate the posture of the humanoid envelope but can also carry out impedance-adaptive matching. It is very important to reproduce the natural pose of human arm for generating human-like behavior for nearby humans in robot applications. Literature ⁹ replicating natural postures of human arms is essential to generate human-like behaviors in robotic applications for humans nearby. This paper proposes a two-phase global minimization algorithm to handle the nonconvexity of the constrained minimization. The proposed approach was validated by replicating the natural arm postures of five right-handed subjects in daily tasks. It can be seen from the recent research results that the evaluation of the dual-arm movement ability is generally in a simplified plane, and the establishment of the model is relatively simple, while the research on the kinematics characteristics of high curvature^10–12 in the daily function of the human arm has made a breakthrough.

Aimed at how the joint movements of the human double arms can be more anthropomorphic on the double-arm robot, this paper describes and accurately analyzes the arm joint kinematics, establishes the arm kinematics model, and then constructs the double-arm dynamics model through the man-machine double-arm mapping, which lays the foundation for the final anthropomorphic accurate control of the double-arm robot. The main contributions of the paper are as follows:

By studying the motion mechanism of the human arm and extracting the joint kinematic chain, a closed solution and joint angle reconstruction method for the inverse kinematics of the arm's natural motion are proposed.

In the rapid reconstruction of the human arm model, the right angle triangle method of arm rotation is used to solve the force and torque of the arm shoulder joint and elbow joint, and finally, the dynamic equations of the position and joint angle of the shoulder joint and elbow joint are obtained.

To solve the problem of inconsistent data matching between human and machine models, an optimal design of robot joints based on the calculation method of the rotation center of arm motion is proposed. Starting from the human morphological structure, the mapping concept is applied to establish a robot model similar to the arm structure and motion of human hands.

Kinematic analysis of human upper limb joint

Human movement mainly depends on joints. The mechanism of each joint of the human body is analyzed according to the anatomical structure of human movement. ¹³ The human arm is composed of two symmetrical arms. Each arm consists of shoulder joint, big arm, elbow joint, forearm, and wrist joint from top to bottom. Each arm has seven degrees of freedom, of which the shoulder joint has three degrees of freedom, the elbow joint has one degree of freedom, and the wrist joint has three degrees of freedom.

Typical joint morphology is mainly on the moving joint surface when describing joint kinematics. The shape of the joint surface is mostly curved or slightly curved, which forms a concave–convex relationship at the joint. The concave–convex relationship of joints enables them to adapt to each other, increase the surface area to dissipate the contact force, and help guide the movement between bones. In this concave–convex relationship, there are three basic forms of motion: rolling, sliding, and rotation. ¹⁴ The research of human arm mainly starts from the shoulder complex. Physiologically, it is believed that the shoulder complex is mainly composed of four movable joints, namely, the Shoulder chest joint, the acromioclavicular joint, the glenohumeral joint, and the sternoclavicular joint with three degrees of freedom. These four parts of joints provide a wide range of possibilities for the movement of both arms so that the human arm can reach the desired target position and operable objects when moving. The elbow joint is a connecting link in the human arm, which is connected with the shoulder joint through the humerus, and the wrist joint is connected with the ulna and radius. The ulna and radius are traditionally called forearms. The elbow joint and forearm form the elbow complex, which consists of three bones and four joints, namely, the humerus, the ulna, the radius and the humeroradio joint, the humerouracial joint, the proximal radioulnar joint, and the distal radioulnar joint. The wrist, also known as the wrist joint, consists of eight carpal bones. As a group, the eight carpal bones are mainly used to connect the forearm and hand. The wrist has a two degree of freedom movement, which is physiologically defined as flexion and extension and ulnar radial deviation.

By analyzing the kinematics of the main joints of the human upper limbs and combining the physiological structure of the human arm, the equivalent kinematic chain of the human double arms can be extracted, as shown in Figure 1. Figure 2 shows the extracted human double-arm kinematic chain. In order to establish the arm kinematics model, the paper refers to the method of establishing the human coordinate system in the International Biomechanics Association. ¹⁵

Figure 1.

Equivalent kinematic chain diagram of human double-arm bone structure.

Figure 2.

Human dual-arm sports chain.

The center of the human double-arm motion chain is in the chest, which is divided into the left arm chain and the right arm chain with the chest as the axis of symmetry. In this paper, the right arm is taken as the research object to segment the chest, the sternoclavicular joint, the shoulder, the elbow, and the wrist, and the joint coordinate system is established, with Figure 2 as a reference. The chest coordinate system can be expressed as

$x_{TH}=\frac{z_{\mathrm{lab}}}{|z_{\mathrm{lab}}|},y_{TH}=z_{TH}\times x_{TH},z_{TH}=\frac{x_{TH}\times (m_r-m_l)}{|x_{TH}\times (m_r-m_l)|}$ , its rotation column vector is $R_{TH}=(x_{TH}{\mathrm{y}}_{TH}{\mathrm{z}}_{TH})$ . The coordinate system of sternoclavicular joint can be expressed as: $x_C=\frac{c_S-c_{TH}}{|c_S-c_{TH}|},y_C=\frac{x_{TH}\times x_C}{|c_S-c_{TH}|},z_C=x_C\times y_C$ , its column vector is expressed as $R_C=(x_C{\mathrm{y}}_C{\mathrm{z}}_C)$ . The three directions of the shoulder coordinate are exactly the same as those of the chest, so the column vector of the shoulder is $R_S=(x_{TH}{\mathrm{y}}_{TH}{\mathrm{z}}_{TH})$ . The coordinates of the elbow joint is expressed as $x_E=\frac{c_S-c_E}{|c_S-c_E|},y_E=\frac{(c_W-c_E)\times (c_S-c_E)}{|(c_W-c_E)\times (c_S-c_E)|},$ $z_E=x_E\times y_E$ , the elbow joint rotation column vector is $R_E=(x_E{\mathrm{y}}_E{\mathrm{z}}_E)$ . The coordinates of the wrist joint is $x_W=\frac{c_E-c_W}{|c_E-c_W|},z_W=\frac{(c_E-c_W)\times (m_{\mathrm{rad}}-m_u)}{|(c_E-c_W)\times (m_{\mathrm{rad}}-m_u)|},y_W=z_w\times x_w$ , the wrist rotation column vector is $R_W=(x_W{\mathrm{y}}_W{\mathrm{z}}_W)$ , where $m_{\mathrm{rad}}$ is the radial position mark of the wrist joint, and $m_u$ is the ulna position mark of the wrist joint. After the coordinates of the system are built, the homogeneous transformation rotation matrix of important joints will be calculated. For the shoulder joint, its direction is determined by the upper arm of the elbow joint and the acromioclavicular joint. Therefore, the rotation matrix of the shoulder joint is $R_{Sh}=R_S^{-1}{R}_E$ , and the rotation matrix of the elbow joint is $R_{El}=R_E^{-1}{\hat{R}}_W{R}_{\mathrm{corr}}$ . Where ${\hat{R}}_W$ is the correction value of wrist joint and $R_{\mathrm{corr}}$ is the correction matrix. The rotation matrix of each joint on the longitudinal axis is decomposed into flexion/extension, abduction/adduction, and rotation. For joint coordinate systems, this means that rotation is first along the y-axis, then the x-axis, and finally, the z-axis. The respective rotation angles are defined as $\beta$ , $\alpha$ , and $\gamma$ .

Calculation of rotation center of arm motion and analysis of force and moment

Calculation of arm motion rotation center

The shoulder, elbow, and wrist joints serve as the rotation center of the upper arm, forearm, and palm respectively. ¹⁶ Next, the least square method is used to estimate the position of the arm rotation center relative to the motion chain of the front rigid arm. For example, it is possible to estimate the center of rotation of the forearm (i.e., the elbow joint), relative to the position of the upper arm rigid body. These points are calculated by projecting the shoulder of the human arm to the reference frame. When the spatial positions of the wrist and elbow, as well as the spatial positions and directions of the palm, are known, the inverse kinematics of the seven joint angles of the arm can be calculated. The movement of human arms in joint space has the characteristics of high dimension and data big, so it is difficult to extract data. The left and right arm mannequins have 14 independent different joint angles. The expression is as follows:

$$Q=[q_1{q}_2\cdots q_{14}]$$

(1)

where Q is the observation matrix, which can be divided into right arm $q_r=[q_1\cdots q_7]$ and left arm $q_l=[q_8\cdots q_{14}]$ according to the left and right arms. In order to obtain a low-dimensional concrete model, refer to the method of principal component analysis. The correlation matrix of initial joint angle data is calculated by observing matrix Q with zero mean variance, and then the correlation matrix is studied in a low-dimensional double-arm relationship by principal component analysis. Then, the noncontributing dimension is removed from the joint data set, and a n dimension variance description close to the original data is retained. This transformation can be written as follows:

$$N_{lr}=UQ$$

(2)

where $q_1,q_2$ are a n-dimensional vector describing the changes of both arms, and U is a matrix of $n\times 14$ . Its eigenvector is calculated through principal component analysis. Similarly, $N_l$ and $N_r$ are column vectors extracted from the columns of $N_{lr}$ . They mainly describe the coordination between arms. Joint observation value: $N_l=U_l{q}_l$ is a low-dimensional representation of the left arm configuration relationship. At this time, through the observation of both arms, the variance is applied to minimize the dimension and eliminate the useless data. In this way, the density function can not only be visualized but also the calculation amount of the controller is reduced, and only the main part of the function is retained. After dimension reduction, the density function gradient (potential function) is established for the human arm, that is, the expression of the dual-arm function relative to the left arm is

$$P(N_l|N_r)={\displaystyle \frac{P(N_l\cap N_r)}{P(N_r)}}$$

(3)

Gaussian mixture model ¹⁷ is used to establish density function gradient and dual-arm coordination. The further operation of continuously representing low-dimensional data sets is supported. Finally, it returns to the joint velocity layer with the gradient of potential function. This process completes the estimation of the rotation center of the entire arm movement.

Analysis of arm rotation force and torque

Suppose that the arm is connected by two frictionless rigid links (upper arm and forearm) through the shoulder and elbow joints, and the shoulder can move freely. The linear acceleration of the moment generated by the shoulder, including the motion of each joint in the equation, is calculated by the following method. The arm model is built on two links, and the shoulder joint can move freely on level $x-y$ . The length of each section is l and the mass is m. To simplify the calculation, the center of mass is CM, the distance from the center of mass to the joint is r, and the inertia matrix is expressed by I. The position of the centroid of each segment in the base coordinate is $p(x,y)$ . The torque generated by the rotation of each joint is recorded as T, and each connecting rod is affected by force F and angular momentum P. For shoulder, Newton Euler method can be used to obtain

$$\begin{array}{rl}& F_s-F_e+m_s{\ddot{p}}_0-m_s{\ddot{p}}_{s_{CM}}=0\\ & P_s-P_e+(p_1-p_{s_{CM}})\times F_e-(p_0-p_{s_{CM}})\times F_s-I_s{\dot{\omega }}_s=0\end{array}$$

(4)

Newton Euler method is also applied to elbow joint

$$\begin{array}{rl}& F_e+m_e{\ddot{p}}_0-m_e{\ddot{p}}_{e_{CM}}=0\\ & P_e-(p_1-p_{e_{CM}})\times F_e-I_e{\dot{\omega }}_e=0\end{array}$$

(5)

In order to obtain the dynamic equation, the joint force is first eliminated and the obvious joint torque in the dynamic equation is separated. Since it is a plane two link, shoulder joint torque $T_s$ and elbow joint torque $T_e$ are equal to the change rates $P_s$ and $P_e$ of angular momentum. The elimination of E and F in equations (4) and (5) can obtain

$$\begin{array}{r}{T}_e-(p_1-p_{e_{CM}})\times m_e{\ddot{p}}_{e_{CM}}+(p_1-p_{e_{CM}})\times m_e{\ddot{p}}_0-I_e{\dot{\omega }}_e=0\end{array}$$

(6)

$$\begin{array}{rl}& T_s-T_e-(p_0-p_{s_{CM}})\times m_s{\ddot{p}}_{s_{CM}}-(p_0-p_1)\times m_e{\ddot{p}}_{e_{CM}}\\ & +(p_0-p_{s_{CM}})\times m_s{\ddot{p}}_0+(p_0-p_1)\times m_e{\ddot{p}}_0-I_s{\dot{\omega }}_s=0\end{array}$$

(7)

Using the independent joint displacement angles ( ${\theta }_s$ and ${\theta }_e$ ), the linear velocity and position vector of shoulder and elbow joints can be further written, that is

$${\omega }_s={\dot{\theta }}_s,{\omega }_e={\dot{\theta }}_s+{\dot{\theta }}_e$$

(8)

$$\begin{array}{rl}& p_{s_{CM}}=\left[\begin{array}{c}{r}_s\cos ({\theta }_s)\\ r_s\sin ({\theta }_s)\end{array}\right],p_{e_{CM}}=\left[\begin{array}{c}{l}_s\cos ({\theta }_s)+r_e\cos ({\theta }_s+{\theta }_e)\\ l_s\sin ({\theta }_s)+r_e\sin ({\theta }_s+{\theta }_e)\end{array}\right]\\ & {\dot{p}}_{s_{CM}}=\left[\begin{array}{c}-r_s{\dot{\theta }}_s\sin ({\theta }_s)\\ r_s{\dot{\theta }}_s\cos ({\theta }_s)\end{array}\right],\\ & {\dot{p}}_{e_{CM}}=\left[\begin{array}{c}-\{l_s\sin ({\theta }_s)+r_e\sin ({\theta }_s+{\theta }_e)\}{\dot{\theta }}_s-r_e\sin ({\theta }_s+{\theta }_e){\dot{\theta }}_e\\ \{l_s\cos ({\theta }_s)+r_e\cos ({\theta }_s+{\theta }_e)\}{\dot{\theta }}_s+r_e\cos ({\theta }_s+{\theta }_e){\dot{\theta }}_e\end{array}\right]\end{array}$$

(9)

By substituting the derivative of equations (6) and (7) with respect to time into equations (8) and (9), the dynamic equations of the positions and joint angles of shoulder and elbow joints can be obtained

$$\begin{array}{rl}& T_s=a{\ddot{\theta }}_s+b{\ddot{\theta }}_e-c{\dot{\theta }}_e^2-2c{\dot{\theta }}_s{\dot{\theta }}_e+\delta \\ & T_e=d{\ddot{\theta }}_e+b{\ddot{\theta }}_s+c{\dot{\theta }}_s^2+\phi \end{array}$$

(10)

where $a=m_s{r}_s^2+I_s+m_e[l_s^2+r_e^2+2l_2{r}_e\cos ({\theta }_e)]+I_e$ , $b=m_e{l}_s{r}_e\cos ({\theta }_e)+m_e{r}_e^2+I_e$ , $\begin{array}{rl}& \delta =[m_s{r}_s\cos ({\theta }_s)+m_e(r_e\cos ({\theta }_s+{\theta }_e)+l_s\cos ({\theta }_s))]\ddot{y}\\ & -\left[\begin{array}{c}{m}_s{r}_s\sin ({\theta }_s)+l_s\sin ({\theta }_s)\\ +m_e(r_e\sin ({\theta }_s+{\theta }_e)\end{array}\right]\end{array}$ , $c=m_e{l}_s{r}_e\sin ({\theta }_e)$ , $\phi =[m_e(r_e\cos ({\theta }_s+{\theta }_e)]\ddot{y}-[m_e{r}_e\sin ({\theta }_s+{\theta }_e)]\ddot{x}$ , $d=m_e{r}_e^2+I_e$ . $m_e$ and $m_s$ represent the mass of the forearm and upper arm, respectively, $x,y$ represent the position of the shoulder, and ${\theta }_s$ and ${\theta }_e$ represent the direction angle of the upper arm and the front.

Man-machine dual-arm mapping and dynamic modeling of dual-arm robot

Based on the human morphological structure, the mapping concept is applied to build a robot model similar to the arm structure and motion of a human hand. Figure 3 shows the human–robot shape mapping.

Figure 3.

Man-machine configuration mapping.

Through the analysis of the mechanism of the human arm, combined with the mapping diagram above, it can be seen that the human arm mainly relies on muscles and bones to complete the movement. They obtain internal and external information through the nervous system and are commanded by the brain. Mapping to the robot is the network communication of the robot, and the processor commands the movement of each driver, joint, and link.

The dynamic model of both arms is established by using the Lagrangian function method. This section is to simplify the 17 degrees of freedom of the upper limb of a two arm robot. The dynamic modeling of the entire robot ignores the two degrees of freedom of the neck and two pairs of wrists with three degrees of freedom, reducing the degree of freedom of the robot from 17 to 9. The simplified D−H coordinate system is shown in Figure 4.

Figure 4.

D-H coordinate system of humanoid robot.

Based on the robot prototype in Figure 3, the dynamic model of the two arm linkage of the humanoid robot is established as shown in Figure 5. Where $q_i$ , ${\theta }_j$ , $l_i$ , and $d_i$ are, respectively, the included angle between the connecting rod i and z-axis, the angle of each joint in the radial plane, the length of the connecting rod, and the distance between the center of mass and the waist joint. If $(x_1,z_1)$ is the position of the waist joint at the base mark, the expression of the center of mass position $(x_i,z_i)$ of each connecting rod is:

$$x_2=x_1+l_1\sin q_1-d_2\sin q_2$$

(11)

$$z_2=z_1+l_1\cos q_1-d_2\cos q_2$$

(12)

$$x_3=x_1+l_1\sin q_1-l_2\sin q_2-d_3\sin q_3$$

(13)

$$z_3=z_1+l_1\cos q_1-l_2\cos q_2-d_3\cos q_3$$

(14)

$$x_4=x_1+l_1\sin q_1-d_4\sin q_4$$

(15)

$$z_4=z_1+l_1\cos q_1-d_4\cos q_4$$

(16)

$$x_5=x_1+l_1\sin q_1-l_4\sin q_4-d_5\sin q_5$$

(17)

$$z_5=z_1+l_1\cos q_1-l_4\cos q_4-d_5\cos q_5$$

(18)

Figure 5.

Simplified model of humanoid robot.

The Lagrangian function L is defined as the kinetic energy K minus the potential energy P, then

$$L=K-P$$

(19)

Similarly, the general expressions of kinetic energy Ki and potential energy Pi of the dual-arm robot model are defined as

$$K_i={\displaystyle \frac{1}{2}{m}_i({\dot{x}}_i^2+{\dot{z}}_i^2)+{\displaystyle \frac{1}{2}{I}_i{\dot{q}}_i^2;P_i=m_{i}g{z}_i}}$$

(20)

where I_i is the moment of inertia of connecting rod i about the center of mass.

The Lagrangian function of the humanoid dual-arm robot is:

$$L=K-P=\sum _{i=2}^n[{\displaystyle \frac{1}{2}{m}_i({\dot{x}}_i^2+{\dot{z}}_i^2)}+{\displaystyle \frac{1}{2}{I}_i{\dot{q}}_i^2-m_{i}g{z}_i]}$$

(21)

The Lagrange equation of the total torque T_i of each link of the humanoid dual-arm robot is:

$$T_i={\displaystyle \frac{d}{dt}{\displaystyle \frac{\mathrm{\partial }L}{\mathrm{\partial }{\dot{q}}_i}-{\displaystyle \frac{\mathrm{\partial }L}{\mathrm{\partial }{q}_i}}}}$$

(22)

where $q_i$ and ${\dot{q}}_i$ represent the angle and angular velocity of the connecting rod, respectively.

The center of mass velocity of each connecting rod is obtained through the derivative of equations (11) to (18), and $q_i$ and ${\dot{q}}_i$ can be obtained by substituting the obtained center of mass velocity value into equation (21). The total torque expression of each link of the humanoid double-arm robot is obtained by derivation of $q_i$ and ${\dot{q}}_i$ , respectively, through equation (22). The expression is as follows:

$$\begin{array}{rl}{T}_1=& [I_1+m_1{d}_1^2+(m_2+m_3+m_4+m_5)l_1^2]{\ddot{q}}_1\\ & -(m_2{d}_2+m_3{l}_2)l_1\cos (q_1-q_2){\ddot{q}}_2\\ & -m_3{d}_3{l}_1\cos (q_1-q_3){\ddot{q}}_3-(m_4{d}_4+m_5{l}_4)\\ & l_1\cos (q_1-q_3){\ddot{q}}_3-m_5{d}_5{l}_1\cos (q_1-q_5){\ddot{q}}_5\\ & -(m_2{d}_2+m_3{l}_2)l_1\sin (q_1-q_2){\dot{q}}_2^2-m_3{d}_3{l}_1\sin (q_1-q_3){\dot{q}}_3^2\\ & +(m_4{d}_4+m_5{l}_4)l_1\sin (q_1-q_4){\dot{q}}_4^2+m_5{d}_5{l}_1\sin (q_1-q_5){\dot{q}}_5^2\\ & -[m_1{d}_1+(m_2+m_3+m_4+m_5)l_1]g\sin q_1\end{array}$$

(23)

$$\begin{array}{rl}{T}_2=& -(m_2{d}_2+m_3{l}_2)l_1\cos (q_1-q_2){\ddot{q}}_1\\ & +(I_2+m_2{d}_2^2+m_3{l}_2^2){\ddot{q}}_2+m_3{d}_3{l}_2\cos (q_2-q_3){\ddot{q}}_3\\ & +(m_2{d}_2+m_3{l}_2)l_1\sin (q_1-q_2){\dot{q}}_1^2\\ & +m_3{l}_2{d}_3\sin (q_2-q_3){\dot{q}}_3^2+(m_2{d}_2+m_3{l}_2)g\sin q_2\end{array}$$

(24)

$$\begin{array}{rl}{T}_3=& -m_3{d}_3{l}_1\cos (q_1-q_3){\ddot{q}}_1+m_3{d}_3{l}_2\cos (q_2-q_3){\ddot{q}}_2\\ & +(I_3+m_3{d}_3^2){\ddot{q}}_3+m_3{d}_3{l}_1\sin (q_1-q_3){\dot{q}}_1^2\\ & +m_3{l}_2{d}_3\sin (q_2-q_3){\dot{q}}_2^2+m_3{d}_{3}g\sin q_3\end{array}$$

(25)

$$\begin{array}{rl}{T}_4=& -(m_4{d}_4+m_5{l}_4)l_1\cos (q_1-q_4){\ddot{q}}_1+(I_4+m_4{d}_4^2+m_5{l}_4^2){\ddot{q}}_4\\ & +m_5{d}_5{l}_4\cos (q_5-q_4){\ddot{q}}_5+(m_4{d}_4+m_5{l}_4)l_1\sin (q_1-q_4){\dot{q}}_1^2\\ & +m_5{l}_4{d}_5\sin (q_5-q_4){\dot{q}}_5^2+(m_4{d}_4+m_5{l}_4)g\sin q_4\end{array}$$

(26)

$$\begin{array}{rl}{T}_5=& -m_5{d}_5{l}_1\cos (q_1-q_5){\ddot{q}}_1+m_5{d}_5{l}_4\cos (q_4-q_5){\ddot{q}}_4\\ & +(I_5+m_5{d}_5^2){\ddot{q}}_5-m_5{d}_5{l}_1\sin (q_1-q_5){\dot{q}}_1^2\\ & +m_5{l}_4{d}_5\sin (q_4-q_5){\dot{q}}_4^2+m_5{d}_{5}g\sin q_5\end{array}$$

(27)

According to the figure and geometric relationship, the angle $q=[q_1{q}_2{q}_3{q}_4{q}_5{]}^T$ of each link and the angle $\theta =[{\theta }_1{\theta }_2{\theta }_3{\theta }_4{\theta }_5{]}^T$ of each joint of the humanoid double-arm robot meet the following relationship

$${\theta }_1=q_1,{\theta }_2=q_1-q_2,{\theta }_3=q_3-q_2,{\theta }_4=q_1-q_4,{\theta }_5=q_5-q_4$$

(28)

However, there is the following relationship between the driving torque $\tau ={\left[\begin{array}{ccccc}{\tau }_1& {\tau }_2& {\tau }_3& {\tau }_4& {\tau }_5\end{array}\right]}^T$ of each joint and the total torque $T={\left[\begin{array}{ccccc}{T}_1& T_2& T_3& T_4& T_5\end{array}\right]}^T$ of each connecting rod: $T=E\cdot \tau$

Therefore, the relationship between the driving torque $\tau$ of each joint and the total torque T of each connecting rod can be obtained:

$$T=\left[\begin{array}{ccccc}1& -1& 0& 0& 0\\ 0& -1& -1& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& -1& -1\\ 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\tau }_1\\ {\tau }_2\\ {\tau }_3\\ {\tau }_4\\ {\tau }_5\end{array}\right]=\left[\begin{array}{c}{T}_1\\ T_2\\ T_3\\ T_4\\ T_5\end{array}\right]$$

(29)

Finally, the relationship between the driving torque $\tau$ of each joint, the angle q of each link, the angular velocity $\dot{q}$ , and the angular acceleration $\ddot{q}$ is obtained as

$$D(q)\cdot \ddot{q}+H(q)\cdot {\dot{q}}^2+G(q)=E\cdot \tau$$

(30)

where $D(q)$ and $H(q)$ are symmetric matrices of $5\times 5$ .

In the model matching of the dual-arm robot from the human arm motion data, the integration of the human arm joint is high, especially the human shoulder joint, which has three degrees of freedom, and the volume of the shoulder joint complex is relatively small. Compared with the robot manipulator, the shoulder joint integration is low, and the shoulder joint volume is large. This makes the shoulder of the robot have many differences in matching with the human arm model after the model is simplified, such as motion space, cooperation space, and dynamic model. The traditional humanoid mechanical arm design uses seven degrees of freedom to establish a kinematic model in series. For the shoulder joint, the three rotating axes of the ball and socket joint intersect at a point, which is called the mechanical rotation center. When rotating, the rotation center is fixed. However, the movement of the physiological rotation center of the human shoulder joint will change with the movement position of the humerus, which makes the data matching of the human–machine model inconsistent. In order to solve this problem, a method based on the calculation of the rotation center of the arm motion is proposed to optimize the design of the robot's dual-arm joints to improve the humanoid degree of the dual-arm robot. Here, mainly through optimizing the structure of the mechanical joint and limiting the range of motion of the arm shoulder joint to match the mechanical arm.

In the process of structural optimization of the mechanical joint, the bending/extension axis and the adduction/extension axis of the offset shoulder joint are used to change the position of the rotation center of the mechanical arm, forming a mechanical rotation center surface to match the movement, so that the mechanical arm has the sliding and rolling functions of the shoulder joint similar to the human body. The specific rotation axis translation and the corresponding rotation center position are shown in Figure 6.

Figure 6.

Schematic diagram of the mechanical rotation center of shoulder rotation axis motion.

In Figure 6, $p_1$ and $p_2$ are the offsets of the extension and retraction axes (Z-axis) and the inclination and camber axes (X-axis), and ${\alpha }_1$ and ${\alpha }_2$ are the offset angles of the two axes and Y-axis, respectively. ${\theta }_1$ and ${\theta }_2$ are the joint angles of forward extension and retraction, and inward inclination and outward inclination. The position relationship of the rotation center point M obtained by three movements of the shoulder joint is as follows:

$$\{\begin{array}{c}{M}_x=-\mathrm{Im}(P)\\ M_y=\mathrm{Re}(N)\\ M_z=\mathrm{Im}(N)\end{array}$$

(31)

where $P=p_2{e}^{(i{\alpha }_2)}+p_2{e}^{[i(\pi +{\alpha }_2+{\theta }_1)]}$ $N=p_1{e}^{(i{\alpha }_2)}+p_1{e}^{[i(\pi +{\alpha }_1-{\theta }_2)]}+$ $\mathrm{Re}(P)e^{(-i{\theta }_2)}$ .

After optimizing the structure of the mechanical joint, it is also necessary to limit the motion range of the mechanical arm joint to ensure that the mechanical arm is closer to human motion in the motion space, cooperation space, and dynamic model.

Experimental verification and analysis

In order to verify the feasibility and effectiveness of the method of quickly reconstructing and mapping the human arm model to the dual-arm robot model in this paper. An optical motion capture platform was used to carry paper boxes on human body. The experimental platform is composed of eight Prime 13 cameras and the Motive: Body software. The accessories include data cables, switches, synchronizers, calibration poles, marker points, network cards, and motion capture clothing. The experimental system can accurately capture the whole body movement of human body. Core hardware and core software are shown in Figure 7. The specific capture motion process is shown in Figure 8.

Figure 7.

Motion capture system experiment platform. (a) Hardware configuration. (b) Software system interface.

Figure 8.

Motion capture experiment of human body carrying objects with both arms.

In Figure 8, A is the experimental initialization state; B means that both arms pick up the object, and both hands hold the object in the center of the body; C refers to the left movement of both arms; D refers to returning to the central position from C state; E means right movement of both arms; F is to return to the central position again, and both arms complete the whole process of one handling. Figure 9 shows the moving object data collected by the motion capture system.

Figure 9.

Acquisition of double-arm motion data.

Human body data are collected based on Prime 13 motion capture system, and joint angles are calculated using space vector method. ¹⁸ Figure 10 shows the trajectory curve of each joint angle of optical motion capture human arm space vector method.

Figure 10.

The trajectory of each joint angle of the arm calculated by the space vector method.

The inverse kinematics relationship between human motion joint angle data and robot model matching is shown in Figure 11.

Figure 11.

Inverse solution of matching data captured from each joint angle of human double-arm motion with robot model (the black solid line is: human motion capture each joint angle track, blue dotted line is: inverse solution of joint angle trajectories by robot matching).

It can be seen from the figure that shoulder joint 1 and 2 match relatively well, elbow joint 4 matches best, and other joints fluctuate greatly, but the maximum error does not exceed joint 7. The reason is that a slight change in the shoulder joint will certainly bring a large change in the end joint, which belongs to the normal category.

In order to verify the effectiveness of the mapping method proposed in this paper, experiments are carried out with a dual-arm robot. The test process is shown in Figure 12. The dual-arm robot in the figure has 14 degrees of freedom, and each wrist joint is equipped with a three-dimensional force sensor. The object to be clamped in the experiment is a paper box, weighing about 500 g, with length, width, and height of 220 mm, 200 mm, and 100 mm, respectively. During the experiment, the contact point between the robot finger and the object does not move relative.

Figure 12.

Motion experiment of object clamped by dual arm.

The tracking results of the test results of the dual-arm clamping and handling carton are shown in Figure 13, and the actual track tracking effect is shown in Figure 13(a):

Figure 13.

Tracking of motion track of object clamped by dual arm in coordination. (a) Motion trace diagram. (b) Motion trajectory tracking error curve.

It can be seen from Figure 13(a) that the three actions performed by the mechanical arm include form moving right to moving left to moving right. The coincidence degree between the tracking track and the planned track at the initial stage is not high. With the movement, its actual track tracking gradually coincides, indicating that its actual tracking effect is good. It can be seen from the error curve in Figure 13(b) that, with the movement, the error of trajectory tracking gradually decreases, indicating that its tracking gradually tends to be stable.

The position tracking effect of the controller under the traditional D-H parameter modeling (TD-HM) and the bionic mechanism mapping modeling (BMMM) in this paper is verified by designing the trajectory in the Cartesian coordinate system of the robot end. The cycloid function is used to plan the trajectory in the Cartesian coordinate system of the robot as follows:

$$\{\begin{array}{c}x(t)=a*\sin \left(\theta +2\pi \mathrm{si}{\mathrm{n}}^2\left({\displaystyle \frac{\pi t}{2T}}\right)\right)\mathrm{\forall }t\in [0,T]\\ y(t)=b*\cos \left(\theta +2\pi \mathrm{si}{\mathrm{n}}^2\left({\displaystyle \frac{\pi t}{2T}}\right)\right)\mathrm{\forall }t\in [0,T]\\ z(t)=0\mathrm{\forall }t\in [0,T]\\ \theta =\arctan \left({\displaystyle \frac{x_0}{y_0}}\right)\end{array}$$

where singledollarx\lpar t \rpar singledollar, singledollar\; y\lpar t \rpar singledollar, and singledollar\; z\lpar t \rpar singledollar represent the displacement of the end of the manipulator along with the X, Y, and Z axes, respectively. Parameters a and b represent the X and Y radius of the ellipse. T is the movement time from the start point to the end point, and singledollar{\rm x}_0singledollar andsingledollar{\rm \; }{\rm y}_0singledollar are the x and y coordinate values of the start point of the robot end. In the simulation process, the initial position of each joint of the manipulator is [0, −0.4, 0, 1.97, 0, −2.7,0] rad. Considering the limitation of the working space of the manipulator, set each parameter in the simulation environment as a = 0.45 m, b = 0.15 m, T = 16 s. The reference track is shown in Figure 14.

Figure 14.

Target trajectory at the end of the manipulator.

In order to compare the tracking performance of the manipulator under different models, the parameters of the controller are set to the same value. The comparison experiment between the TD-HM and the BMMM control in this paper is done (Figure 15).

Figure 15.

Joint reference track.

Figure 16 shows the comparison between joint space trajectory tracking and reference trajectory diagram 15 of the manipulator under the TD-HM and the BMMM in this paper. Figure 16 shows that the bionic mechanism mapping model has higher position tracking accuracy and compliance than the TD-HM. Therefore, in the humanoid robot system, the BMMM can achieve relatively high position accuracy and compliance.

Figure 16.

Comparison curve of joint trajectory tracking between the traditional D-H parameter model of 7-DOF manipulator and the bionic mechanism mapping model. (a) Joint 1. (b) Joint 2. (c) Joint 3. (d) Joint 4. (e) Joint 5. (f) Joint 6. (g) Joint 7.

Further validate the experiment, we can call it force observer (FO)-BMMM, a nonlinear FO in BMMM is employed to estimate the varying payload. Another set of experiment called FS-BMMM was conducted by replacing nonlinear observer with a six-axis wrist force/torque sensor (FS) in BMMM. Those experiments are applied to measure the varying force values in the Z direction and test the effective of the BMMM. The trajectory tacking of end-effector of the manipulator in FO-BMMM, FS-BMMM, and the desired path generated by equation (30) is shown in Figure 17. Figures 18 and 19 show the six-dimensional force measurement and nonlinear observation curves, respectively.

Figure 17.

The desired path and the end-effecter trajectories measured by the 3D NDI in cartesian space during the experiments.

Figure 18.

The force measured by six-axis wrist force/torque sensor.

Figure 19.

The joint force estimated by the nonlinear observer.

The comparative errors between the trajectory executed by the manipulator in the FO-BMMM experiment and the desired path generated by equation (30) are shown in Figure 20(a). The errors between the trajectory executed by the manipulator in the FS-BMMM experiment and the desired path are shown in Figure 20(b). Notice that Figure (20) clearly shows that the error is larger along the Z axis, since the varying payload in Z direction is considered. In addition, also the comparison showed that the tracking error in Figure 20(b) is larger by a few millimeters than in Figure 20(a), which is acceptable considering the measuring errors and other uncertainties.

Figure 20.

The errors between the desired trajectory of the end-effecter point that measured during experiences. (a) The FO-BMMM experiment. (b) The FS-BMMM experiment.

The above experiments fully demonstrate the feasibility of the biomimetic mechanism mapping model and the effectiveness of tracking trajectories, especially under the condition of payload, which can also be stable.

Conclusions

Inspired by biology, this paper proposed a method to map the data collected from human arm movements to the humanoid robot arms. This mapping not only reproduced the motion model configuration of human arms but also transformed its anthropomorphic compliant actions into robot actions. In the process of model mapping, we analyzed the calculation of the rotation center of human arm motion and the force and torque and explored the method of anthropomorphic configuration in the quantitative model.

This method guided the right angle triangulation method of arm rotation to solve the force and torque of the shoulder and elbow joints of the arm when quickly reconstructing the human arm model and obtained the dynamic equations of the positions and joint angles of the shoulder and elbow joints. The optimization design based on the calculation method of the rotation center of the arm motion was proposed to solve the problem of inconsistent data matching of the human–machine model. Simulation and experiment were used for testing and comparison, involving humanoid robot arms. The results showed that although the human arm and the robot model mapped from the arm model have different kinematic trajectories, the matching error between the captured data of each joint angle of the human arm motion and the robot model motion is very small. In future work, we will try to extend the proposed method to different manipulators and more complex objects.