第二章运动学 – 2.3类ur，二三四轴轴线平行解析解

大家好，欢迎来到IT知识分享网。

这边对另一种符合Piper准则的机器人构型进行运动学分析，与传统的工业机器人相比，此类机械臂关注焦点在于关节二、三、四的轴线平行，并不像传统机械臂种的关节四、五、六轴线交于一点，但是根据Piper准则来说，存在解析解。这也是大多数协作机械臂的基础，下面就对类UR机械臂进行建模分析。

图2.3.1 类ur机械臂DH模型

根据2.1节相关建模方式，可以对类ur类型的机械模型建立如上图所示，建立的时候可以根据一边建坐标系一边写DH模型的方法来写出相应的DH模型表，如下：

（二）、正解

根据上一章节，我们同理可以得到正解：

${{}_{~}^{0}T_{1}} = R_x(\alpha_0)\cdot T_x(a_0)\cdot R_z(\theta_1)\cdot T_z(d_1)$ (2-3-1)

同理可以得到 ${{}_{~}^{1}T_{2}}$ 、 ${{}_{~}^{2}T_{3}}$ 、 ${{}_{~}^{3}T_{4}}$ 、 ${{}_{~}^{4}T_{5}}$ 、 ${{}_{~}^{5}T_{6}}$ 、将这些矩阵相乘即可得到 ${{}_{~}^{0}T_{6}}$ 、

${{}_{~}^{0}T_{6}} = {{}_{~}^{0}T_{1}} \cdot {{}_{~}^{1}T_{2}} \cdot {{}_{~}^{2}T_{3}} \cdot {{}_{~}^{3}T_{4}} \cdot {{}_{~}^{4}T_{5}} \cdot {{}_{~}^{5}T_{6}}$ (2-3-2)

关于奇异，协作机械臂的奇异跟前一章节的奇异类似，公式也差不多，唯一的区别在于肘关节的臂型有所区别，肘关节奇异定义是，关节二轴、三轴、四轴共线，此时关节二的逆解无法解出，其实也就是关节三为0时奇异。根据奇异，可分8种臂型，只不过，一般协作机械臂正逆解，因为是解析解的缘故，一般不会提及8种臂型，但奇异仍然存在。

（三）、逆解（解析解）

我们先将每个关节的 ${{}_{~}^{i-1}T_{i}}$ 矩阵写出来如下：

${{}_{~}^{0}T_{1}} = ~\begin{bmatrix} \begin{matrix} c_1 \\ s_1 \\ 0 \\0 \end{matrix} & \begin{matrix} \begin{matrix} -s_1 \\ c_1 \\ 0 \\ 0\end{matrix} & \begin{matrix} 0 \\ 0 \\ 1\\ 0\end{matrix} & \begin{matrix} \begin{matrix} 0 \\ 0 \\ d_1 \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$ ${{}_{~}^{1}T_{2}} = ~\begin{bmatrix} \begin{matrix} -c_2 \\ 0 \\ -s_2 \\0 \end{matrix} & \begin{matrix} \begin{matrix} s_2 \\ 0 \\ -c_2 \\ 0\end{matrix} & \begin{matrix} 0 \\ -1 \\ 0\\ 0\end{matrix} & \begin{matrix} \begin{matrix} 0 \\ 0 \\ 0 \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$

${{}_{~}^{2}T_{3}} = ~\begin{bmatrix} \begin{matrix} c_3 \\ s_3 \\ 0 \\0 \end{matrix} & \begin{matrix} \begin{matrix} -s_3 \\ c_3 \\ 0 \\ 0\end{matrix} & \begin{matrix} 0 \\ 0 \\ 1\\ 0\end{matrix} & \begin{matrix} \begin{matrix} a_2 \\ 0 \\ 0 \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$ ${{}_{~}^{3}T_{4}} = ~\begin{bmatrix} \begin{matrix} c_4 \\ s_4 \\ 0 \\0 \end{matrix} & \begin{matrix} \begin{matrix} -s_4 \\ c_4 \\ 0 \\ 0\end{matrix} & \begin{matrix} 0 \\ 0 \\ 1\\ 0\end{matrix} & \begin{matrix} \begin{matrix} a_3 \\ 0 \\ d_4 \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$

${{}_{~}^{4}T_{5}} = ~\begin{bmatrix} \begin{matrix} 1\\ 0\\ 0 \\0 \end{matrix} & \begin{matrix} \begin{matrix} 0 \\ 0\\ 1 \\ 0\end{matrix} & \begin{matrix} 0 \\ 1 \\ 0\\ 0\end{matrix} & \begin{matrix} \begin{matrix} 0\\ d_5 \\ 0 \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$ ${{}_{~}^{5}T_{6}} = ~\begin{bmatrix} \begin{matrix} -c_6 \\ 0 \\ -s_6 \\0 \end{matrix} & \begin{matrix} \begin{matrix} s_6 \\ 0 \\ -c_6 \\ 0\end{matrix} & \begin{matrix} 0 \\ -1 \\ 0\\ 0\end{matrix} & \begin{matrix} \begin{matrix} 0 \\ -d_6 \\ 0 \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$ (2-3-3)

令

${{}_{~}^{0}T_{6}}=~\begin{bmatrix} \begin{matrix} n_x \\ n_y \\ n_z \\0 \end{matrix} & \begin{matrix} \begin{matrix} o_x \\ o_y \\ o_z \\ 0\end{matrix} & \begin{matrix} a_x \\ a_y \\ a_z\\ 0\end{matrix} & \begin{matrix} \begin{matrix} p_x \\ p_y \\ p_z \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$ (2-3-4)

有等式如下：

${{}_{~}^{0}T_{1}}^{-1}\cdot{{}_{~}^{0}T_{6}} \cdot{{}_{~}^{5}T_{6}}^{-1}= {{}_{~}^{1}T_{5}}={{}_{~}^{1}T_{2}}\cdot{{}_{~}^{2}T_{3}}\cdot{{}_{~}^{3}T_{4}}\cdot{{}_{~}^{4}T_{5}}$ (2-3-5)

（1）、解 $\theta_1$

将式（2-3-3）带入到（2-3-5），通过比较就可以发现，两边矩阵第二行第四列有如下等式：

(2-3-6)

整理得：

(2-3-7)

解出：

$\theta_1= ±atan2(-d_4,sqrt(m^2+n^2-d_4^2 ))-atan2(m,n)$ (2-3-8)

（2）、解 $\theta_5$

同时经过（2-3-5）可以得到第二行第一列，第二行第三列有如下等式：

(2-3-9)

那么可得到：

$\theta_5= atan2(s_5,c_5 ),s_5= \pm \sqrt{(1-c_5^2 )}$ (2-3-10)

（3）、解 $\theta_6$

s_5=s_6 m-c_6 n”> (2-3-11)

0 = -c_6 m-s_6n”> (2-3-12)

所以：

$\theta_6=atan2(n,m)+atan2(s_5,0)$ (2-3-13)

（4）、解 $\theta_3$

解完4、5、6轴之后，将上式再次相乘：

${{}_{~}^{0}T_{1}}^{-1}\cdot{{}_{~}^{0}T_{6}} \cdot{{}_{~}^{5}T_{6}}^{-1}\cdot{{}_{~}^{4}T_{5}}^{-1}= {{}_{~}^{1}T_{4}}={{}_{~}^{1}T_{2}}\cdot{{}_{~}^{2}T_{3}}\cdot{{}_{~}^{3}T_{4}}$ (2-3-14)

得到(2-3-14)第一行第四列，和第三行第四列有：

$c_2a_2+a_3 c_{23}= x,$ $s_2 a_2+a_3 s_{23}= y$ (2-3-15)

这里的x和y表示一大长串的关于1、5、6轴的表达式

将（2-3-15）的两式平方可得：

“> $\theta_3 =arccos((x^2+y^2-a_2^2-a_3^3)/2(a_2a_3))$ (2-3-16)

（5）、解 $\theta_2$

将 $\theta_2$ 带入(2-3-15)，得到二元一次方程组如下：

$\begin{bmatrix} \begin{matrix} c_2\\ s_2\end{matrix} \end{bmatrix} \cdot ~\begin{bmatrix} \begin{matrix} A\\ C\end{matrix} & \begin{matrix} \begin{matrix} B\\ D\end{matrix} \end{matrix} \end{bmatrix}=\begin{bmatrix} \begin{matrix} M\\ N\end{matrix} \end{bmatrix}$ (2-3-17)

其中

得到

$\theta_2=atan2(s_2,c_2)$ (2-3-18)

（6）、解 $\theta_4$

式子(2-3-15)中根据矩阵第一行第二列和第三行第二列可以得到：

$-c_6 (c_1 o_x+o_y s_1 )-s_6 (c_1 n_x+n_y s_1 )=s_{234}$

$c_6 o_z+n_z s_6=c_{234}$ (2-3-19)

$\theta_4=atan2(s_{234},c_{234} )-\theta_3-\theta_2$ (2-3-20)

注意，在求 $\theta_6$ 时，(2-3-13)可以看出，当 $\theta_5=0$ 或者 $n\pi$ 时，矩阵某些项为0，在求别的关节时存在正余弦值为0，需要注意，也可以理解为奇异。

另外，根据上面公式，可以看出， $\theta_1$ 有两个解， $\theta_5$ 有四个解， $\theta_6$ 有四个解， $\theta_2,\theta_3,\theta_4$ 有八个解，最后关于解的选取，这里没有一个确定的方法，可根据实际情况，可以由最小角度或者最小能量，或者臂型来选取，甚至还有些是旋转±360度之后，可以产生更多选项的解，所以这块可以根据厂商的要求自己来定义。

完成上述计算之后，程序如下：

公式推导：

syms nx ny nz ox oy oz ax ay az px py pz; syms d1 a2 a3 d4 d5 d6; syms q1 q2 q3 q4 q5 q6; syms c1 c2 c3 c4 c5 c6 s1 s2 s3 s4 s5 s6 syms T01 T12 T23 T34 T45 T56; syms T01_1 T12_1 T23_1 T34_1 T45_1 T56_1; %% q1:rotz T01 = [c1 -s1 0 0;s1 c1 0 0;0 0 1 d1; 0 0 0 1]; T01_1 = [c1 s1 0 0; -s1 c1 0 0;0 0 1 -d1; 0 0 0 1]; %% q2:rotx(90)*rotz(pi+theta); T12 = [-c2 s2 0 0;0 0 -1 0; -s2 -c2 0 0;0 0 0 1;]; T12_1 = [-c2 0 -s2 0; s2 0 -c2 0;0 -1 0 0;0 0 0 1]; %% q3:transx(a2)*rotz(theta) T23 = [c3 -s3 0 a2;s3 c3 0 0;0 0 1 0; 0 0 0 1;]; %% q4:transx(a3)*rotz(theta) T34 = [c4 -s4 0 a3; s4 c4 0 0;0 0 1 d4;0 0 0 1;]; %% q5:rotx(-90)*rotz(theta)*transz(d5); T45 = [c5 -s5 0 0; 0 0 1 d5;-s5 c5 0 0; 0 0 0 1;]; T45_1 = [c5 0 s5 0;s5 0 c5 0;0 1 0 -d5; 0 0 0 1]; %% q6:rotx(pi/2)*rotz(theta+pi)*transz(d6); T56 = [-c6 s6 0 0; 0 0 -1 -d6;-s6 -c6 0 0;0 0 0 1]; T56_1 = [-c6 0 -s6 0;s6 0 -c6 0;0 -1 0 -d6;0 0 0 1]; T06 = [nx ox ax px;ny oy ay py;nz oz az pz; 0 0 0 1]; %公式：(2-3-5) T01_1*T06*T56_1 T12*T23*T34*T45 %公式：(2-3-14) T01_1*T06*T56_1*T45_1 T12*T23*T34 程序2.3.1协作机械臂公式推导

程序：正解：

function [T] = ur5_fkine(q,DH,q_option) if strcmp(q_option,'deg') q = (q+DH.theta)*pi/180.0; elseif strcmp(q_option,'rad') ; else q = (q+DH.theta)*pi/180.0; end if strcmp(q_option,'deg') alpha= DH.alpha*pi/180.0; elseif strcmp(q_option,'rad') ; else alpha= DH.alpha*pi/180.0; end T01 = create_link(q(1),DH.d(1),alpha(1),DH.a(1),'rad'); T12 = create_link(q(2),DH.d(2),alpha(2),DH.a(2),'rad'); T23 = create_link(q(3),DH.d(3),alpha(3),DH.a(3),'rad'); T34 = create_link(q(4),DH.d(4),alpha(4),DH.a(4),'rad'); T45 = create_link(q(5),DH.d(5),alpha(5),DH.a(5),'rad'); T56 = create_link(q(6),DH.d(6),alpha(6),DH.a(6),'rad'); T = T01*T12*T23*T34*T45*T56; end 程序2.3.2协作机械臂正解

反解：

function [q] = ur5_ikine(DH,T) d1 = DH.d(1); d4 = DH.d(4); d5 = DH.d(5); d6 = DH.d(6); a2 = DH.a(3); a3 = DH.a(4); nx = T(1,1); ny = T(2,1); nz = T(3,1); ox = T(1,2); oy = T(2,2); oz = T(3,2); ax = T(1,3); ay = T(2,3); az = T(3,3); px = T(1,4); py = T(2,4); pz = T(3,4); m = py-d6*ay;n = d6*ax-px; %q1: q1(1) = atan2(-d4,sqrt(m^2 + n^2-d4^2))-atan2(m,n); q1(2) = atan2(-d4,-sqrt(m^2 + n^2-d4^2))-atan2(m,n); qa = atan2(-ax,ay)*180/pi %q5: c1 = cos(q1(1));s1 = sin(q1(1)); c5 = ax*s1 - ay*c1;s5_1 = sqrt(1-c5^2);s5_2 = -sqrt(1-c5^2); q5(1) = atan2(s5_1,c5); q5(2) = atan2(s5_2,c5); c1 = cos(q1(2));s1 = sin(q1(2)); c5 = ax*s1 - ay*c1;s5_1 = sqrt(1-c5^2);s5_2 = -sqrt(1-c5^2); q5(3) = atan2(s5_1,c5); q5(4) = atan2(s5_2,c5); %q6: c1 = cos(q1(1));s1 = sin(q1(1));s5 = sin(q5(1)); m = c1*oy - ox*s1;n = c1*ny - nx*s1; q6(1) = atan2(n,m)+atan2(s5,0); s5 = sin(q5(2)); q6(2) = atan2(n,m)+atan2(s5,0); c1 = cos(q1(2));s1 = sin(q1(2));s5 = sin(q5(1)); m = c1*oy - ox*s1;n = c1*ny - nx*s1; q6(3) = atan2(n,m)+atan2(s5,0); s5 = sin(q5(2)); q6(4) = atan2(n,m)+atan2(s5,0); [q3(1),q3(2)] = cal_formula_q3(T,DH,sin(q1(1)),cos(q1(1)),sin(q6(1)),cos(q6(1))); [q3(3),q3(4)] = cal_formula_q3(T,DH,sin(q1(1)),cos(q1(1)),sin(q6(2)),cos(q6(2))); [q3(5),q3(6)] = cal_formula_q3(T,DH,sin(q1(2)),cos(q1(2)),sin(q6(3)),cos(q6(3))); [q3(7),q3(8)] = cal_formula_q3(T,DH,sin(q1(2)),cos(q1(2)),sin(q6(4)),cos(q6(4))); [q2(1)] = cal_formula_q2(T,DH,sin(q1(1)),cos(q1(1)),sin(q6(1)),cos(q6(1)),sin(q3(1)),cos(q3(1))); [q2(2)] = cal_formula_q2(T,DH,sin(q1(1)),cos(q1(1)),sin(q6(1)),cos(q6(1)),sin(q3(2)),cos(q3(2))); [q2(3)] = cal_formula_q2(T,DH,sin(q1(1)),cos(q1(1)),sin(q6(2)),cos(q6(2)),sin(q3(3)),cos(q3(3))); [q2(4)] = cal_formula_q2(T,DH,sin(q1(1)),cos(q1(1)),sin(q6(2)),cos(q6(2)),sin(q3(4)),cos(q3(4))); [q2(5)] = cal_formula_q2(T,DH,sin(q1(2)),cos(q1(2)),sin(q6(3)),cos(q6(3)),sin(q3(5)),cos(q3(5))); [q2(6)] = cal_formula_q2(T,DH,sin(q1(2)),cos(q1(2)),sin(q6(3)),cos(q6(3)),sin(q3(6)),cos(q3(6))); [q2(7)] = cal_formula_q2(T,DH,sin(q1(2)),cos(q1(2)),sin(q6(4)),cos(q6(4)),sin(q3(7)),cos(q3(7))); [q2(8)] = cal_formula_q2(T,DH,sin(q1(2)),cos(q1(2)),sin(q6(4)),cos(q6(4)),sin(q3(8)),cos(q3(8))); [q4(1)] = cal_formula_q4(T,sin(q1(1)),cos(q1(1)),sin(q6(1)),cos(q6(1)),q2(1),q3(1)); [q4(2)] = cal_formula_q4(T,sin(q1(1)),cos(q1(1)),sin(q6(1)),cos(q6(1)),q2(2),q3(2)); [q4(3)] = cal_formula_q4(T,sin(q1(1)),cos(q1(1)),sin(q6(2)),cos(q6(2)),q2(3),q3(3)); [q4(4)] = cal_formula_q4(T,sin(q1(1)),cos(q1(1)),sin(q6(2)),cos(q6(2)),q2(4),q3(4)); [q4(5)] = cal_formula_q4(T,sin(q1(2)),cos(q1(2)),sin(q6(3)),cos(q6(3)),q2(5),q3(5)); [q4(6)] = cal_formula_q4(T,sin(q1(2)),cos(q1(2)),sin(q6(3)),cos(q6(3)),q2(6),q3(6)); [q4(7)] = cal_formula_q4(T,sin(q1(2)),cos(q1(2)),sin(q6(4)),cos(q6(4)),q2(7),q3(7)); [q4(8)] = cal_formula_q4(T,sin(q1(2)),cos(q1(2)),sin(q6(4)),cos(q6(4)),q2(8),q3(8)); q = zeros(8,6); q = [q1(1) q2(1) q3(1) q4(1) q5(1) q6(1); q1(1) q2(2) q3(2) q4(2) q5(1) q6(1); q1(1) q2(3) q3(3) q4(3) q5(2) q6(2); q1(1) q2(4) q3(4) q4(4) q5(2) q6(2); q1(2) q2(5) q3(5) q4(5) q5(3) q6(3); q1(2) q2(6) q3(6) q4(6) q5(3) q6(3); q1(2) q2(7) q3(7) q4(7) q5(4) q6(4); q1(2) q2(8) q3(8) q4(8) q5(4) q6(4);]; q = q*180/pi; return; end function [q,q_] = cal_formula_q3(T,DH,s1,c1,s6,c6) d1 = DH.d(1); d4 = DH.d(4); d5 = DH.d(5); d6 = DH.d(6); a2 = DH.a(3); a3 = DH.a(4); nx = T(1,1); ny = T(2,1); nz = T(3,1);ox = T(1,2); oy = T(2,2); oz = T(3,2);ax = T(1,3); ay = T(2,3); az = T(3,3);px = T(1,4); py = T(2,4); pz = T(3,4); x = c1*px + py*s1 + d5*(c6*(c1*ox + oy*s1) + s6*(c1*nx + ny*s1)) - d6*(ax*c1 + ay*s1); y = pz - d1 - az*d6 + d5*(c6*oz + nz*s6); q = acos((x^2+y^2-a2^2-a3^2)/(2*a2*a3));q_ = -acos((x^2+y^2-a2^2-a3^2)/(2*a2*a3)); end function [q] = cal_formula_q2(T,DH,s1,c1,s6,c6,s3,c3) d1 = DH.d(1); d4 = DH.d(4); d5 = DH.d(5); d6 = DH.d(6); a2 = DH.a(3); a3 = DH.a(4); nx = T(1,1); ny = T(2,1); nz = T(3,1);ox = T(1,2); oy = T(2,2); oz = T(3,2);ax = T(1,3); ay = T(2,3); az = T(3,3);px = T(1,4); py = T(2,4); pz = T(3,4); x = c1*px + py*s1 + d5*(c6*(c1*ox + oy*s1) + s6*(c1*nx + ny*s1)) - d6*(ax*c1 + ay*s1); y = pz - d1 - az*d6 + d5*(c6*oz + nz*s6); A = a2+a3*c3;B = -a3*s3; C = s3*a3; D = a2+a3*c3; M = -x;N = -y; c2 = (D*M-N*B)/(A*D-B*C); s2 = (A*N-C*M)/(A*D-B*C); q = atan2(s2,c2); end function [q] = cal_formula_q4(T,s1,c1,s6,c6,q2,q3) nx = T(1,1); ny = T(2,1); nz = T(3,1);ox = T(1,2); oy = T(2,2); oz = T(3,2); s234 = -c6*(c1*ox + oy*s1) - s6*(c1*nx + ny*s1) ; c234 = c6*oz + nz*s6 ; q = atan2(s234,c234) - q2 - q3; end 程序2.3.3协作机械臂逆解

免责声明：本站所有文章内容,图片，视频等均是来源于用户投稿和互联网及文摘转载整编而成，不代表本站观点，不承担相关法律责任。其著作权各归其原作者或其出版社所有。如发现本站有涉嫌抄袭侵权/违法违规的内容,侵犯到您的权益，请在线联系站长,一经查实,本站将立刻删除。本文来自网络,若有侵权，请联系删除，如若转载，请注明出处：https://haidsoft.com/150359.html

第二章运动学 – 2.3类ur，二三四轴轴线平行解析解

相关推荐

发表回复