Analytical Solutions of the Inverse Kinematics of a Humanoid Robot

Open Live Script

This example shows how to derive analytical solutions for theinverse kinematicsof the head chain of a humanoid robot.

Step 1: Define Parameters

Describe the kinematics of the head-chain link (the link between the torso and the head) of the NAO humanoid robot [1] using theDenavit-Hartenberg (DH)parameters and notations based on a study by Kofinas et al. [2]. The following transformation defines the head-chain link.

$T = A_{B a s e, 0} T_{0, 1} T_{1, 2} R_{x} R_{y} A_{2, H e a d}$

where:

$A_{B a s e, 0}$ is the translation from the base (torso) to the joint or reference frame 0
$T_{0, 1}$ is the orientation of reference 1 relative to 0
$T_{1, 2}$ is the orientation of reference 2 relative to 1
$R_{x}$ is the roll rotation
$R_{y}$ is the pitch rotation
$A_{2, H e a d}$ is the translation from reference 2 to the end-effector point (the head)

$T (1 : 3, 4)$ defines the coordinates of the head, which are $x c$ , $y c$ , $z c$ ．

In this example, you analytically solve the inverse kinematics problem by returning all orientations of individual joints in the head-chain link given head coordinates ofxc,yc, andzcwithin the reachable space. Then, you convert the analytical results to purely numeric functions for efficiency.

Solving analytically (when doing so is possible) lets you perform computations in real time and avoid singularities, which cause difficulty for numerical methods.

Step 2: Define Forward Kinematics of Head Chain Using DH Parameters

The functiongetKinematicChain返回的特定运动链NAO robot in terms of symbolic variables. For details ongetKinematicChain, see the Helper Functions section.

kinChain ='head';dhInfo = getKinematicChain(kinChain); T = dhInfo.T

T=
                  
                    $(\begin{array}{c} r_{11} & r_{12} & r_{13} & xc \\ r_{21} & r_{22} & r_{23} & yc \\ r_{31} & r_{32} & r_{33} & zc \\ 0 & 0 & 0 & 1 \end{array})$

Express the forward kinematics transformation matrixTas the following sequence of products:T= ABase0*T01*T12*Rx*Ry*A2Head．Define the individual matrices as follows.

Specify the translation matrix from the torso (base) to joint 0.

ABase0 = dhInfo.ABase0

ABase0 =
                  
                    $(\begin{array}{c} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & NeckOffsetZ \\ 0 & 0 & 0 & 1 \end{array})$

Specify the transformation matrix from joint 0 to joint 1.

T01 = dhInfo.T01

T01 =
                  
                    $(\begin{array}{c} \cos (θ_{1}) & - \sin (θ_{1}) & 0 & 0 \\ \sin (θ_{1}) & \cos (θ_{1}) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array})$

Specify the transformation matrix from joint 1 to joint 2.

T12 = dhInfo.T12

T12 =
                  
                    $(\begin{array}{c} \cos (θ_{2} - \frac{π}{2}) & - \sin (θ_{2} - \frac{π}{2}) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ - \sin (θ_{2} - \frac{π}{2}) & - \cos (θ_{2} - \frac{π}{2}) & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array})$

Specify the roll rotation matrix.

Rx = dhInfo.Rx

Rx =
                  
                    $(\begin{array}{c} 1 & 0 & 0 & 0 \\ 0 & 0 & - 1 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array})$

Specify the pitch rotation matrix.

Ry = dhInfo.Ry

Ry =
                  
                    $(\begin{array}{c} 0 & 0 & 1 & 0 \\ 0 & 1 & 0 & 0 \\ - 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array})$

Specify the translation matrix from joint 2 to the head.

A2Head = dhInfo.A2Head

A2Head =
                  
                    $(\begin{array}{c} 1 & 0 & 0 & CameraX \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & CameraZ \\ 0 & 0 & 0 & 1 \end{array})$

The known parameters of this problem areCameraX,CameraZ,NeckOffsetZ, and the positionsxc,yc, andzc．The unknown parameters are $θ_{1}$ and $θ_{2}$ ．After finding $θ_{1}$ and $θ_{2}$ , you can compute the individual transformations ofT．The robot can then achieve the desired positionxc,yc,zc．

Although you can see these parameters in the transformation matrices, they do not exist as variables in the MATLAB base workspace. This is because these parameters originate from a function. Functions do not use the base workspace. Each function workspace is separate from the base workspace and all other workspaces to protect the data integrity. Thus, to use these variables outside of the functiongetKinematicChain, usesymsto create them.

symsCameraXCameraZNeckOffsetZxcyczctheta_1theta_2real;

Step 3: Find Algebraic Solutions for $θ_{1}$ and $θ_{2}$

Rewrite the equationT= ABase0*T01*T12*Rx*Ry*A2Head, separating the terms that describe the torso and head movements of the robot:inv(T01)*inv(ABase0)*T = T12*Rx*Ry*A2Head．简化equatio的左右n, and define equations of interest matching the expressions for coordinate positions.

LHS = simplify(inv(T01)*inv(ABase0)*T); RHS = simplify(T12*Rx*Ry*A2Head); eqns = LHS(1:3,end) - RHS(1:3,end)

eqns =
                  
                    $(\begin{array}{c} xc \cos (θ_{1}) - CameraZ \sin (θ_{2}) - CameraX \cos (θ_{2}) + yc \sin (θ_{1}) \\ yc \cos (θ_{1}) - xc \sin (θ_{1}) \\ zc - NeckOffsetZ - CameraZ \cos (θ_{2}) + CameraX \sin (θ_{2}) \end{array})$

This system of equations contains two variables for which you want to find solutions, $θ_{1}$ and $θ_{2}$ ．However, the equations also imply that you cannot arbitrarily choosexc,yc, andzc．Therefore, also considerycas a variable. All other unknowns of the system are symbolic parameters.

This example follows a typical algebraic approach for solving inverse kinematics problems [3]. The idea is to get a compact representation of the solution, where the expression of each variable is in terms of parameters and variables for which you already solved the equations. As a first step, find either $θ_{1}$ or $θ_{2}$ in terms of the parameters. Then, express the other variable in terms of the known variable and parameters. To do this, start by identifying the equation that has only one variable.

intersect(symvar(eqns(1)),[theta_1,theta_2,yc])

ans =
                   $(\begin{array}{c} θ_{1} & θ_{2} & yc \end{array})$

intersect(symvar(eqns(2)),[theta_1,theta_2,yc])

ans =
                   $(\begin{array}{c} θ_{1} & yc \end{array})$

intersect(symvar(eqns(3)),[theta_1,theta_2,yc])

ans =
                   $θ_{2}$

The third equation contains only one variable, $θ_{2}$ ．Solve this equation first.

[theta_2_sol,theta_2_params,theta_2_conds] =．..解决(theta_2方程式(3),'Real',true,'ReturnConditions',true)

theta_2_sol =
                  
                    $(\begin{array}{c} 2 π k - 2 : (\frac{CameraX - \sqrt{{CameraX}^{2} + {CameraZ}^{2} - {NeckOffsetZ}^{2} + 2 NeckOffsetZ zc - {zc}^{2}}}{CameraZ - NeckOffsetZ + zc}) \\ 2 π k - 2 : (\frac{CameraX + \sqrt{{CameraX}^{2} + {CameraZ}^{2} - {NeckOffsetZ}^{2} + 2 NeckOffsetZ zc - {zc}^{2}}}{CameraZ - NeckOffsetZ + zc}) \end{array})$

theta_2_params =
                   $k$

theta_2_conds =
                  
                    $(\begin{array}{c} k \in Z \land CameraZ + zc \neq NeckOffsetZ \land {(NeckOffsetZ - zc)}^{2} \leq {CameraX}^{2} + {CameraZ}^{2} \\ k \in Z \land CameraZ + zc \neq NeckOffsetZ \land {(NeckOffsetZ - zc)}^{2} \leq {CameraX}^{2} + {CameraZ}^{2} \end{array})$

The solutions have an additive $2 π k$ term with the parameter $k$ ．Without loss of generalization, you can set $k$ equal to 0 in the solutions and conditions.

theta_2_sol = subs(theta_2_sol,theta_2_params,0)

theta_2_sol =
                  
                    $(\begin{array}{c} - 2 : (\frac{CameraX - \sqrt{{CameraX}^{2} + {CameraZ}^{2} - {NeckOffsetZ}^{2} + 2 NeckOffsetZ zc - {zc}^{2}}}{CameraZ - NeckOffsetZ + zc}) \\ - 2 : (\frac{CameraX + \sqrt{{CameraX}^{2} + {CameraZ}^{2} - {NeckOffsetZ}^{2} + 2 NeckOffsetZ zc - {zc}^{2}}}{CameraZ - NeckOffsetZ + zc}) \end{array})$

forp = 1:numel(theta_2_conds) theta_2_conds(p) = simplify(subs(theta_2_conds(p),theta_2_params,0));end

Now solve for the variables $θ_{1}$ andycin terms of the variable $θ_{2}$ and other symbolic parameters.

[theta_1_sol,yc_sol,yc_theta_1_params,yc_theta_1_conds] =．..solve(eqns(1:2),theta_1,yc,'Real',true,'ReturnConditions',true);

Display the solutions for $θ_{1}$ andyc．

theta_1_sol

theta_1_sol =
                  
                    $\begin{array}{l} (\begin{array}{c} 2 π k - σ_{1} \\ σ_{2} + 2 π k \\ 2 π k - σ_{2} \\ 2 π k - \frac{π}{2} \\ σ_{1} + 2 π k \\ 2 : (x) + 2 π k \\ \frac{π}{2} + 2 π k \\ 2 π k - \frac{π}{2} \\ \frac{π}{2} + 2 π k \end{array}) \\ where \\ σ_{1} = 2 : (\frac{\sqrt{(CameraX + xc) (CameraX - xc)}}{CameraX - xc}) \\ σ_{2} = 2 : (\frac{\frac{σ_{3}}{σ_{5} + 1} + \frac{σ_{5} σ_{3}}{σ_{5} + 1}}{σ_{4}}) \\ σ_{3} = \sqrt{- (xc - CameraX + CameraX σ_{5} + xc σ_{5} - 2 CameraZ \tan (\frac{θ_{2}}{2})) σ_{4}} \\ σ_{4} = CameraX + xc - CameraX σ_{5} + xc σ_{5} + 2 CameraZ \tan (\frac{θ_{2}}{2}) \\ σ_{5} = {\tan (\frac{θ_{2}}{2})}^{2} \end{array}$

yc_sol

yc_sol =
                  
                    $\begin{array}{l} (\begin{array}{c} \sqrt{(CameraX + xc) (CameraX - xc)} \\ σ_{1} \\ - σ_{1} \\ CameraX \\ - \sqrt{(CameraX + xc) (CameraX - xc)} \\ 0 \\ σ_{2} \\ - σ_{2} \\ - CameraX \end{array}) \\ where \\ σ_{1} = \frac{\sqrt{- (xc - CameraX + σ_{3} + xc {\tan (\frac{θ_{2}}{2})}^{2} - σ_{4}) (CameraX + xc - σ_{3} + xc {\tan (\frac{θ_{2}}{2})}^{2} + σ_{4})}}{{\tan (\frac{θ_{2}}{2})}^{2} + 1} \\ σ_{2} = \frac{- σ_{3} + σ_{4} + CameraX}{{\tan (\frac{θ_{2}}{2})}^{2} + 1} \\ σ_{3} = CameraX {\tan (\frac{θ_{2}}{2})}^{2} \\ σ_{4} = 2 CameraZ \tan (\frac{θ_{2}}{2}) \end{array}$

The solutions depend on two parameters, $k$ and $x$ ．

yc_theta_1_params

yc_theta_1_params =
                   $(\begin{array}{c} k & x \end{array})$

The solutions have an additive $2 π k$ term. Without loss of generalization, you can set $k$ equal to 0 in the solution fortheta_1_soland the conditionsyc_theta_1_conds．

theta_1_sol = simplify(subs(theta_1_sol,yc_theta_1_params,[0,0]))

theta_1_sol =
                  
                    $\begin{array}{l} (\begin{array}{c} - 2 : (\frac{\sqrt{(CameraX + xc) (CameraX - xc)}}{CameraX - xc}) \\ σ_{1} \\ - σ_{1} \\ - \frac{π}{2} \\ 2 : (\frac{\sqrt{(CameraX + xc) (CameraX - xc)}}{CameraX - xc}) \\ 0 \\ \frac{π}{2} \\ - \frac{π}{2} \\ \frac{π}{2} \end{array}) \\ where \\ σ_{1} = 2 : (\frac{\sqrt{(CameraX \cos (θ_{2}) - xc + CameraZ \sin (θ_{2})) (xc + CameraX \cos (θ_{2}) + CameraZ \sin (θ_{2}))}}{xc + CameraX \cos (θ_{2}) + CameraZ \sin (θ_{2})}) \end{array}$

forp = 1:numel(yc_theta_1_conds) yc_theta_1_conds(p) = simplify(subs(yc_theta_1_conds(p),yc_theta_1_params,[0,0]));end

A similar substitution is not required foryc_solsince there is no dependency on the parameters.

intersect(symvar(yc_sol),yc_theta_1_params)

ans = Empty sym: 1-by-0

Step 4: Verify the Solutions

Starting with an arbitrary set of known numeric values for $θ_{1}$ and $θ_{2}$ , compute the numeric values of the end-effector positionsxc,yc, andzcwith forward kinematics. Then, work backwards fromxc,yc, andzcto compute all possible numeric values for $θ_{1}$ and $θ_{2}$ using the inverse kinematics expressions from the previous computation. Verify that one set of the inverse solutions matches the starting set of numeric values for $θ_{1}$ and $θ_{2}$ ．

Set the fixed parameters for the robot. The values in this example are for illustration purposes only.

CameraXValue = 53.9; CameraZValue = 67.9; NeckOffsetZValue = -5;

Using forward computations, find the end-effector positions corresponding to the values $θ_{1}$ and $θ_{2}$ ．

Tfk = ABase0*T01*T12*Rx*Ry*A2Head; xyz = Tfk(1:3,end);

Create a symbolic function for these positions.

xyzFunc = symfun(subs(xyz,[CameraX,CameraZ,NeckOffsetZ],．..[CameraXValue,CameraZValue,NeckOffsetZValue]),[theta_1,theta_2]);

For forward kinematics, specify two variablestheta_1_knownandtheta_2_knownthat contain an arbitrary starting set of values for $θ_{1}$ and $θ_{2}$ ．

theta_1_known = [pi/4,pi/6,pi/8,pi/10]; theta_2_known = [pi/8,-pi/12,pi/16,-pi/10]; numPts = numel(theta_1_known); num_theta_2 = numel(theta_2_sol); num_theta_1 = numel(theta_1_sol);

Note that there are potentiallynum_theta_1solutions for eachnum_theta_2solution.

Use the following sequence of operations to verify the solutions.

Loop over(theta_1_known,theta_2_known)．For each point, compute the end positionsx_known,y_known, andz_known, which are then "known".
Loop over the solutions for $θ_{2}$ corresponding tox_knownandz_known．For each solution, check to see if the corresponding conditioncond_theta_2is valid. If the condition is valid, compute the corresponding numeric solutiontheta_2_derived．
Loop over the solutions for $θ_{1}$ corresponding tox_known,z_known, andtheta_2_derived．For each solution, check to see if the corresponding conditioncond_theta_1is valid. If the condition is valid, check to see ify_derivednumerically matchesy_knownwithin relative and absolute tolerances through the conditioncond_yc．If this condition is valid, then computetheta_1_derived．
Collect the results in a tableTfor display purposes.

T= table([],[],[],[],'VariableNames',{'theta_1_known','theta_2_known',．..'theta_1_derived','theta_2_derived'});forix1 = 1:numPts xyz_known = double(xyzFunc(theta_1_known(ix1),theta_2_known(ix1))); x_known = xyz_known(1); y_known = xyz_known(2); z_known = xyz_known(3);forix2 = 1:num_theta_2% theta_2 loopcond_theta_2 = subs(theta_2_conds(ix2),[CameraX,CameraZ,NeckOffsetZ,xc,zc],．..[CameraXValue,CameraZValue,NeckOffsetZValue,x_known,z_known]);ifisAlways(cond_theta_2)% if theta_2 is validtheta_2_derived = subs(theta_2_sol(ix2),[CameraX,CameraZ,NeckOffsetZ,xc,zc],．..[CameraXValue,CameraZValue,NeckOffsetZValue,x_known,z_known]); theta_2_derived = double(theta_2_derived);forix3 = 1:num_theta_1% theta_1 loopcond_theta_1 = subs(yc_theta_1_conds(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],．..[CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]);ifisAlways(cond_theta_1)% if theta_1 is validy_derived = subs(yc_sol(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],．..[CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]); y_derived = double(y_derived); cond_yc = abs(y_known - y_derived) < max(100*eps,1e-6*abs(y_known));% check roundingifisAlways(cond_yc)% if yc is validtheta_1_derived = subs(theta_1_sol(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],．..[CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]); theta_1_derived = double(theta_1_derived); T = vertcat(T,table(theta_1_known(ix1),theta_2_known(ix1),theta_1_derived,．..theta_2_derived,'VariableNames',T.Properties.VariableNames));endendendendendendT

T=8×4 tabletheta_1_known theta_2_known theta_1_derived theta_2_derived _____________ _____________ _______________ _______________ 0.7854 0.3927 0.7854 0.3927 0.7854 0.3927 -2.3562 -1.7346 0.5236 -0.2618 0.5236 -0.2618 0.5236 -0.2618 -2.618 -1.0801 0.3927 0.19635 0.3927 0.19635 0.3927 0.19635 -2.7489 -1.5383 0.31416 -0.31416 0.31416 -0.31416 0.31416 -0.31416 -2.8274 -1.0278

Observe that there are two possible solution pairs(theta_1_derived,theta_2_derived)obtained using inverse kinematics for each pair of starting angles(theta_1_known,theta_2_known)．One set of the inverse solutions matches the starting angles.

References

SoftBank Robotics.NAO．https://www.softbankrobotics.com/emea/en/nao．
Kofinas, N., E. Orfanoudakis, and M. G. Lagoudakis. "Complete Analytical Inverse Kinematics for NAO．" In2013 13th International Conference on Autonomous Robot Systems．Lisbon, Portugal: Robotica, 2013.
Kendricks, K. "Solving the Inverse Kinematic Robotics Problem: A Comparison Study of the Denavit-Hartenberg Matrix and Groebner Basis Theory．" Ph.D. Thesis. Auburn University, Auburn, AL, 2007.

Helper Functions

functionkinChain = getKinematicChain(link)% This function returns the kinematic chain of the NAO humanoid robot% represented using Denavit-Hartenberg (DH) parameters.% The function uses as a reference the paper: Complete analytical inverse% kinematics for NAO by Kofinas, N., Orfanoudakis, E., Lagoudakis, M.G.,% 2013 13th International Conference on Autonomous Robot Systems% (Robotica), Publication Year: 2013.ifnargin < 1 link ='head';end% Notation: A, R, and T are the translation, rotation, and DH% transformation matrices, respectively.% Specify DH parameters for the desired end configuration.symsr11r12r13r21r22r23r31r32r33xcyczcreal;R = [r11 r12 r13;r21 r22 r23;r31 r32 r33]; kinChain.T = [R,[xc;yc;zc];0,0,0,1];switchlinkcase'head'% Kinematic chain from torso (base) to head.symsCameraXCameraZNeckOffsetZreal;% Translation from torso (base) to joint 0.ABase0 = getA([0 0 NeckOffsetZ]);% Translation from joint 2 to head.A2Head = getA([CameraX 0 CameraZ]);% Transformation from joint 0 to joint 1.% theta_1 is the rotation angle corresponding to the 0-1 link.symstheta_1real;alpha1 = 0; a1 = 0; d1 = 0; T01 = getT(a1,alpha1,d1,theta_1);% Transformation from joint 1 to joint 2.% theta_2 is the rotation angle corresponding to the 1-2 link.symstheta_2real;piby2 = str2sym('pi/2'); d2 = 0; a2 = 0; alpha2 = -piby2; T12 = getT(a2,alpha2,d2,theta_2-piby2);% Rx is the roll rotation.Rx = getR('x',piby2);% Ry is the pitch rotation.Ry = getR('y',piby2);% Capture the kinematic chain as a string.% The transformation is from the base to the head.kinChain.DHChain ='ABase0*T01*T12*Rx*Ry*A2Head';kinChain.ABase0 = ABase0; kinChain.T01 = T01; kinChain.T12 = T12; kinChain.Rx = Rx; kinChain.Ry = Ry; kinChain.A2Head = A2Head;case'lefthand'% Kinematic chain from torso to left hand.symsShoulderOffsetYElbowOffsetYShoulderOffsetZreal;ABase0 = getA([0 (ShoulderOffsetY+ElbowOffsetY) ShoulderOffsetZ]); symsHandOffsetXLowerArmLengthreal;AEnd4 =木屐(((HandOffsetX + LowerArmLength) 0 0]);piby2 = str2sym('pi/2'); symstheta_1real;alpha1 = -piby2; a1 = 0; d1 = 0; T01 = getT(a1,alpha1,d1,theta_1); symstheta_2real;d2 = 0; a2 = 0; alpha2 = piby2; T12 = getT(a2,alpha2,d2,theta_2-piby2); symsUpperArmLengththeta_3real;d3 = UpperArmLength; a3 = 0; alpha3 = -piby2; T32 = getT(a3,alpha3,d3,theta_3); symstheta_4real;d4 = 0; a4 = 0; alpha4 = piby2; T43 = getT(a4,alpha4,d4,theta_4); Rz = getR('z',piby2); kinChain.DHChain ='ABase0*T01*T12*T32*T43*Rz*AEnd4';kinChain.ABase0 = ABase0; kinChain.T01 = T01; kinChain.T12 = T12; kinChain.T32 = T32; kinChain.T43 = T43; kinChain.Rz = Rz; kinChain.AEnd4 = AEnd4;endendfunctionA = getA(vec)% This function returns the translation matrix.A = [1 0 0 vec(1); 0 1 0 vec(2); 0 0 1 vec(3); 0 0 0 1];endfunctionR = getR(orientation,theta)% This function returns the rotation matrix.switchorientationcase'x'R = [1 0 0 0; 0 cos(theta) -sin(theta) 0; 0 sin(theta) cos(theta) 0 0 0 0 1];case'y'R = [cos(theta) 0 sin(theta) 0; 0 1 0 0; -sin(theta) 0 cos(theta) 0; 0 0 0 1];case'z'R = [cos(theta) -sin(theta) 0 0; sin(theta) cos(theta) 0 0; 0 0 1 0; 0 0 0 1];endendfunctionT= getT(a,alpha,d,theta)% This function returns the Denavit-Hartenberg (DH) matrix.T= [cos(theta),-sin(theta),0,a; sin(theta)*cos(alpha),cos(theta)*cos(alpha),-sin(alpha),-sin(alpha)*d; sin(theta)*sin(alpha),cos(theta)*sin(alpha),cos(alpha),cos(alpha)*d; 0,0,0,1];end