第一次作业¶
Word报告：
坐标系建立描述、公式推导过程，给出\(r_A3=f(l_i,\theta_i)\)的解析表达式
用Matlab编写计算程序，带入给定数值后，得出计算结果，程序放在 附录中。
思考\(\theta_i=f(l_i,r_A3)\)如何推导得到?
notation¶
矩阵定义¶
(1)¶\[\begin{split}
R_x(\theta) =
\begin{bmatrix}
R_{xx} & R_{xy} & R_{xz} & T_x \\
R_{yx} & R_{yy} & R_{yz} & T_y \\
R_{zx} & R_{zy} & R_{zz} & T_z \\
0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
拆解¶
X轴旋转
(2)¶\[\begin{split}
R_x(\theta) =
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & \cos \theta_3 & -\sin \theta_3 & 0 \\
0 & \sin \theta_3 & \cos \theta_3 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
Y轴旋转
(3)¶\[\begin{split}
R_y(\theta) =
\begin{bmatrix}
\cos \theta_1 & & 0 \sin \theta_1 & 0 \\
0 & 1 & 0 & 0 \\
-\sin \theta_1 & 0 & \cos \theta_1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
Z轴旋转
(4)¶\[\begin{split}
R_z(\theta) =
\begin{bmatrix}
\cos \theta_1 & -\sin \theta_1 & 0 & 0 \\
\sin \theta_1 & \cos \theta_1 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
位移
(5)¶\[\begin{split}
T(x, y, z) =
\begin{bmatrix}
1 & 0 & 0 & x \\
0 & 1 & 0 & y \\
0 & 0 & 1 & z \\
0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
示例过程¶
给定参数¶
(6)¶\[\begin{split}
l_1 = 240, \quad l_2 = 60, \quad l_3 = 150, \quad l_4 = 70 \\
\theta_1 = 35^\circ, \quad \theta_2 = 40^\circ, \quad \theta_3 = -25^\circ
\end{split}\]
A0-A1¶
（theta1，l1，l2）
(7)¶\[\begin{split}
R_z(\theta_1) = \begin{bmatrix}
\cos \theta_1 & -\sin \theta_1 & 0 & 0 \\
\sin \theta_1 & \cos \theta_1 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
(8)¶\[\begin{split}
T(-l_2, 0, l_1) = \begin{bmatrix}
1 & 0 & 0 & -l_2 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & l_1 \\
0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
A1-A2¶
(9)¶\[\begin{split}
R_y(\theta_2) = \begin{bmatrix}
\cos \theta_2 & 0 & \sin \theta_2 & 0 \\
0 & 1 & 0 & 0 \\
-\sin \theta_2 & 0 & \cos \theta_2 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
(10)¶\[\begin{split}
T(0, l_3, 0) = \begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & l_3 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
A2-A3¶
(11)¶\[\begin{split}
R_y(\theta_3) = \begin{bmatrix}
\cos \theta_3 & 0 & \sin \theta_3 & 0 \\
0 & 1 & 0 & 0 \\
-\sin \theta_3 & 0 & \cos \theta_3 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
(12)¶\[\begin{split}
T(0, l_4, 0) = \begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & l_4 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
组合¶
(13)¶\[
T_1 = R_z(\theta_1) \cdot T(-l_2, 0, l_1)
\]
(14)¶\[
T_2 = R_y(\theta_2) \cdot T(0, l_3, 0)
\]
(15)¶\[
T_3 = R_y(\theta_3) \cdot T(0, l_4, 0)
\]
(16)¶\[
T_{total} = T_1 \cdot T_2 \cdot T_3
\]
(17)¶\[
T_{total} = R_z(\theta_1)T(-l_2,0,l_1)R_y(\theta_2)T(0,l_3,0)R_y(\theta_3)T(0,l_4,0)
\]
结果¶
(18)¶\[\begin{split}
T_{12}=\begin{bmatrix}
\cos(\theta_1) & -\sin(\theta_1)\cos(\theta_2) & \sin(\theta_1)\sin(\theta_2) & l_2\cos(\theta_1) - l_3\sin(\theta_1)\cos(\theta_2) \\
\sin(\theta_1) & \cos(\theta_1)\cos(\theta_2) & -\sin(\theta_2)\cos(\theta_1) & l_2\sin(\theta_1) + l_3\cos(\theta_1)\cos(\theta_2) \\
0 & \sin(\theta_2) & \cos(\theta_2) & l_1 + l_3\sin(\theta_2) \\
0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
(19)¶\[\begin{split}
T_{23} =
{\begin{bmatrix}
\cos(\theta_1) & \sin(\theta_1)\sin(\theta_2)\sin(\theta_3) - \sin(\theta_1)\cos(\theta_2)\cos(\theta_3) & \sin(\theta_1)\sin(\theta_2)\cos(\theta_3) + \sin(\theta_1)\sin(\theta_3)\cos(\theta_2) & -l_2\cos(\theta_1) - l_3\sin(\theta_1)\cos(\theta_2) + l_4\sin(\theta_1)\sin(\theta_2)\sin(\theta_3) - l_4\sin(\theta_1)\cos(\theta_2)\cos(\theta_3) \\
\sin(\theta_1) & -\sin(\theta_2)\sin(\theta_3)\cos(\theta_1) + \cos(\theta_1)\cos(\theta_2)\cos(\theta_3) & -\sin(\theta_2)\cos(\theta_1)\cos(\theta_3) - \sin(\theta_3)\cos(\theta_1)\cos(\theta_2) & -l_2\sin(\theta_1) + l_3\cos(\theta_1)\cos(\theta_2) - l_4\sin(\theta_2)\sin(\theta_3)\cos(\theta_1) + l_4\cos(\theta_1)\cos(\theta_2)\cos(\theta_3) \\
0 & \sin(\theta_2)\cos(\theta_3) + \sin(\theta_3)\cos(\theta_2) & -\sin(\theta_2)\sin(\theta_3) + \cos(\theta_2)\cos(\theta_3) & l_1 + l_3\sin(\theta_2) + l_4\sin(\theta_2)\cos(\theta_3) + l_4\sin(\theta_3)\cos(\theta_2) \\
0 & 0 & 0 & 1
\end{bmatrix}}
\end{split}\]
(20)¶\[\begin{split}
\text{Position} = \begin{bmatrix} x \\ y \\ z \end{bmatrix} = T_{03}(1:3, 4)
\end{split}\]
matlab/octave¶
DH in octave or matlab¶
clear
clc
pkg load symbolic
% Define symbolic variables
syms theta1 theta2 theta3 l1 l2 l3 l4
#theta1=35*pi/180;theta2=40*pi/180;theta3=-25*pi/180;l1=240;l2=60;l3=150;l4=70
% Rotation matrix around Z-axis
Rz = @(theta) [[cos(theta), -sin(theta), 0, 0];
[sin(theta), cos(theta), 0, 0];
[0, 0, 1, 0];
[0, 0, 0, 1]];
% Rotation matrix around Y-axis
Ry = @(theta) [[cos(theta), 0, sin(theta), 0];
[0, 1, 0, 0];
[-sin(theta), 0, cos(theta), 0];
[0, 0, 0, 1]];
% Rotation matrix around X-axis
Rx = @(theta) [[1, 0, 0, 0];
[0, cos(theta), -sin(theta), 0];
[0, sin(theta), cos(theta), 0];
[0, 0, 0, 1]];
% Translation matrix
T = @(x, y, z) [[1, 0, 0, x];
[0, 1, 0, y];
[0, 0, 1, z];
[0, 0, 0, 1]];
% Calculate the total transformation matrix
T_total = Rz(theta1) * T(-l2, 0, l1) * Rx(theta2) * T(0, l3, 0) * Rx(theta3) * T(0, l4, 0)
% Extract the position vector
position = T_total(1:3, 4);
% Display the position
disp('position');
disp(position);
python¶
DH in python¶
import sympy as sp
theta1, theta2, theta3 = sp.symbols('theta1 theta2 theta3')
l1, l2, l3, l4 = sp.symbols('l1 l2 l3 l4')
def Rz(theta):
return sp.Matrix([
[sp.cos(theta), -sp.sin(theta), 0, 0],
[sp.sin(theta), sp.cos(theta), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
def Ry(theta):
return sp.Matrix([
[sp.cos(theta), 0, sp.sin(theta), 0],
[0, 1, 0, 0],
[-sp.sin(theta), 0, sp.cos(theta), 0],
[0, 0, 0, 1]])
def Rx(theta):
return sp.Matrix([
[1, 0, 0, 0],
[0, sp.cos(theta), -sp.sin(theta), 0],
[0, sp.sin(theta), sp.cos(theta), 0],
[0, 0, 0, 1]])
def T(x,y,z):
return sp.Matrix([
[1,0,0,x],
[0,1,0,y],
[0,0,1,z],
[0,0,0,1]])
T_total = Rz(theta1)*T(-l2,0,l1)*Rx(theta2)*T(0,l3,0)*Rx(theta3)*T(0,l4,0)
position = T_total[:3,3]
print("position")
sp.pprint(position)