六軸機器人正解

採用的是John H.Craig在【機器人學導論】中的改進DH模型

1.六軸座標系

2.DH參數

i	$\alpha_{i-1}$	$a_{i-1}$	$d_i$	$\theta_i$初值
1	0	0	0	0
2	-90	$a_1$	0	-90
3	0	$a_2$	0	-90
4	-90	$a_3$	$d_4$	0
5	90	0	0	0
6	-90	0	$d_6$	0

3.相鄰關節變換矩陣

3.1 基礎變換

$_{i}^{i-1}T= \left[ \begin{matrix} c\theta_{i} & -s\theta_{i} & 0 & a_{i-1} \\ s\theta_{i}c\alpha_{i-1} & c\theta_{i}c\alpha_{i-1} & -s\alpha_{i-1} & -s\alpha_{i-1}d_{i} \\ s\theta_{i}s\alpha_{i-1} & c\theta_{i}s\alpha_{i-1} & c\alpha_{i-1} & c\alpha_{i-1}d_{i} \\ 0& 0 & 0 & 1 \end{matrix} \right] \tag{3-1}$

3.2 各個軸間變換

按照第二節給出的DH參數。其中$c_{i}=c\theta_{i}$,$s_{i}=s\theta_{i}$
$_{1}^{0}T= \left[ \begin{matrix} c_{1} & -s_{1} & 0 & a_{0} \\ s_{1} & c_{1} & 0 & 0 \\ 0& 0 & 1 &0 \\ 0& 0 & 0 &1 \end{matrix} \right] \tag{3-2}$

$_{2}^{1}T= \left[ \begin{matrix} c_{2} & -s_{2} & 0 & a_{1} \\ 0 & 0 & 1 & 0 \\ -s_{2}& -c_{2} & 0 &0 \\ 0& 0 & 0 &1 \end{matrix} \right] \tag{3-3}$

$_{3}^{2}T= \left[ \begin{matrix} c_{3} & -s_{3} & 0 & 0 \\ s_{3}& c_{3} & 0 &0 \\ 0 & 0 & 1 & 0 \\ 0& 0 & 0 &1 \end{matrix} \right] \tag{3-4}$

$_{4}^{3}T= \left[ \begin{matrix} c_{4} & -s_{4} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ -s_{4}& -c_{4} & 0 &0 \\ 0& 0 & 0 &1 \end{matrix} \right] \tag{3-5}$

$_{5}^{4}T= \left[ \begin{matrix} c_{5} & -s_{5} & 0 & 0 \\ 0 & 0 & -1 & 0 \\ s_{5}& c_{5} & 0 &0 \\ 0& 0 & 0 &1 \end{matrix} \right] \tag{3-6}$

$_{6}^{5}T= \left[ \begin{matrix} c_{6} & -s_{6} & 0 & 0 \\ 0 & 0 & 1 & d_6 \\ -s_{6}& -c_{6} & 0 &0 \\ 0& 0 & 0 &1 \end{matrix} \right] \tag{3-7}$

3.3 末端變換矩陣

$\begin{aligned} _{6}^{0}T&={_{1}^{0}T}{_{2}^{1}T}{_{3}^{2}T}{_{4}^{3}T}{_{5}^{4}T}{_{6}^{5}T} \\ &= \left[ \begin{matrix} r_{11} & r_{12} & r_{13} & x \\ r_{21} & r_{22} & r_{23} & y \\ r_{31}& r_{32} & r_{33} &z \\ 0& 0 & 0 &1 \end{matrix} \right] \tag{3-8} \end{aligned}$

$\begin{aligned} x&=c_{1}(a_{1}+a_{3}c_{23}-d_{4}s_{23}+a_{2}c_{2})-d_{6}(s_{23}c_{1}c_{5}+s_{1}s_{4}s_{5}+s_{5}c_{4}c_{1}c_{23}) \\ y&=s_{1}(a_{1}+a_{3}c_{23}-d_{4}s_{23}+a_{2}c_{2})-d_{6}(s_{23}s_{1}c_{5}-c_{1}s_{4}s_{5}+s_{5}c_{4}s_{1}c_{23}) \\ z&=-a_{3}s_{23}-d_{4}c_{23}-a_{2}s_{2}+d_{6}(s_{23}c_{4}s_{5}-c_{23}c_{5}) \\ r_{11}&= c_{1}[c_{23}(c_{4}c_{5}c_{6}-s_{4}s_{6})-s_{23}s_{5}c_{6}]+s_{1}(s_4c_5c_6+c_4s_6)\\ r_{21}&= s_{1}[c_{23}(c_{4}c_{5}c_{6}-s_{4}s_{6})-s_{23}s_{5}c_{6}]-c_1(s_4c_5c_6+c_4s_6)\\ r_{31}&= s_{23}(-c_4c_5c_6+s_4s_6)-c_{23}s_5c_6\\ r_{12}&= c_{1}[c_{23}(-c_{4}c_{5}s_{6}-s_{4}c_{6})+s_{23}s_{5}s_{6}]+s_{1}(-s_4c_5s_6+c_4c_6)\\ r_{22}&= s_{1}[c_{23}(-c_{4}c_{5}s_{6}-s_{4}c_{6})+s_{23}s_{5}s_{6}]-c_{1}(-s_4c_5s_6+c_4c_6)\\ r_{32}&= s_{23}(c_4c_5s_6+s_4c_6)+c_{23}s_5s_6 \\ r_{13}&= -c_{1}(c_{23}c_4s_5+s_{23}c_5)-s_1s_4s_5\\ r_{23}&= -s_{1}(c_{23}c_4s_5+s_{23}c_5)+c_1s_4s_5\\ r_{33}&= s_{23}c_4s_5-c_{23}c_5\\ \beta&=atan2(-r_{31},\sqrt{r_{11}^2+r_{21}^2}) \\ \alpha&=atan2(r_{21}/c_{\beta},r_{11}/c_{\beta}) \\ \gamma&=atan2(r_{32}/c_{\beta},r_{33}/c_{\beta}) \tag{3-9} \end{aligned}$

若$\beta=±90°$,則
$\begin{aligned} \beta&=±90°\\ \alpha&=0° \\ \gamma&=±atan2(r_{12},r_{22}) \end{aligned}$

4. matlab符號表達式推算

function []=test()
syms t1 t2 t3 t4 t5 t6
syms a1 a2 a3 d4 d6
a0=0;a4=0;a5=0;
d1=0;d2=0;d3=0;d5=0;

T0_1 = Trans(0,0,0,t1)
T1_2 = Trans(-90,a1,0,t2)
T2_3 = Trans(0,a2,0,t3)
T3_4 = Trans(-90,a3,d4,t4)
T4_5 = Trans(90,0,0,t5)
T5_6 = Trans(-90,0,d6,t6)

T0_6 = T0_1*T1_2*T2_3*T3_4*T4_5*T5_6;

x=simplify(T0_6(1,4));
y=simplify(T0_6(2,4));
z=simplify(T0_6(3,4));
[rz ry rx]=Rot2Eular(T0_6(1:3,1:3));

end

function [T]=Trans(alpha,a,d,t)
T=[cos(t) -sin(t) 0 a;...
    cosd(alpha)*sin(t) cos(t)*cosd(alpha) -sind(alpha) -sind(alpha)*d;...
    sind(alpha)*sin(t) cos(t)*sind(alpha) cosd(alpha) cosd(alpha)*d;...
    0 0 0 1];
end

function [z y x] = Rot2Eular(r)
    z = simplify(atan2(-r(1,1),(r(1,1)^2+r(2,1)^2)^0.5));
    y= simplify(atan2(r(2,1)/cos(z),r(1,1)/cos(z)));
    x= simplify(atan2(r(3,2)/cos(z),r(3,3)/cos(z)));
end

5.matlab正解計算

function [pose]=fkine(rob,q)
    pose.trans=[0 0 0];
    pose.quat=[1 0 0 0];
    s1=sin(q(1));c1=cos(q(1));
    s2=sin(q(2));c2=cos(q(2));
    s3=sin(q(3));c3=cos(q(3));
    s4=sin(q(4));c4=cos(q(4));
    s5=sin(q(5));c5=cos(q(5));
    s6=sin(q(6));c6=cos(q(6));
    s23=sin(q(2)+q(3));c23=cos(q(2)+q(3));
    a1=rob.a1;a2=rob.a2;a3=rob.a3;
    d6=rob.d6;
    
    tmp1 = a1+a3*c23-rob.d4*s23+a2*c2;
    tmp2 = s23*c1*c5+s1*s4*s5+s5*c4*c1*c23;
    tmp3 = s23*s1*c5-c1*s4*s5+s5*c4*s1*c23;
    
    %% XYZ
    pose.trans(1)=c1*tmp1-d6*tmp2;
    pose.trans(2)=s1*tmp1-d6*tmp3;
    pose.trans(2)=-a3*s23-d4*c23-a2*s2+d6*(s23*c4*s5-c23*c5);
    
    %% quat
    r11 = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)+s1*(s4*c5*c6+c4*s_6);
    r21 = s1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-c1*(s4*c5*c6+c4*s6);
    r31 = s23*(-c4*c5*c6+s4*s6)-c23*s5*c6;
    r12 = c1*(c23*(-c4*c5*s6-s4*c6)+s23*s5*s6)+s1*(-s4*c5*s6+c4*c6);
    r22 = s1*(c23*(-c4*c5*s6-s4*c6)+s23*s5*s6)-c1*(-s4*c5*s6+c4*c6);
    r32 = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
    r13 = -c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
    r23 = -s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
    r33 = s23*c4*s5-c23*c5;
    R=[r11 r12 r13;r21 r22 r23;r31 r32 r33];
    pose.quat=rotm2quat(R);    
end