step1.0 拿到Gluon机械臂参数
step2.0 建立DH-M阵
PS：
1）根据我的建模方法，
t
h
e
t
a
2
theta_2
theta2与
t
h
e
t
a
4
theta_4
theta4有偏置
2）末端长是因为我建模时候把夹爪长度也算进去了
a
i
−
1
α
i
−
1
d
i
θ
i
1
0
0
0.1015
θ
1
2
0
−
π
2
0.0792
θ
2
−
π
2
3
0.173
0
−
0.0792
θ
3
4
0.173
0
0.0792
θ
4
+
π
2
5
0
π
2
0.0792
θ
5
6
0
−
π
2
0.225
θ
6
\begin{array} {cll} \hline & a_{i-1} & \alpha_{i-1} & d_i & \theta_i\\ \hline 1 & 0 & 0 & 0.1015 & \theta_1\\ 2 & 0 & -\frac{\pi}{2} & 0.0792& \theta_2-\frac{\pi}{2}\\ 3 & 0.173 & 0 & -0.0792& \theta_3\\ 4 & 0.173 & 0 & 0.0792& \theta_4+\frac{\pi}{2} \\ 5 & 0 & \frac{\pi}{2} & 0.0792& \theta_5 \\ 6 & 0 & -\frac{\pi}{2} & 0.225& \theta_6 \\ \hline \end{array}
123456ai−1000.1730.17300αi−10−2π002π−2πdi0.10150.0792−0.07920.07920.07920.225θiθ1θ2−2πθ3θ4+2πθ5θ6
step3.0 输入到matlab中使用rbt工具建立机械臂模型
%% 新建机械臂
% by_xiAmAodou 2023-11-26
function robot = creat_robot_arm()
% 定义各连杆
L(1)=Link([0, 0.1015, 0, 0, 0],'modified','qlim',[-140,140]/180*pi);
L(2)=Link([0, 0.0792, 0, -pi/2, 0],'modified','qlim',[-90,90]/180*pi);
L(3)=Link([0, -0.0792, 0.173, 0, 0],'modified','qlim',[-140,140]/180*pi);
L(4)=Link([0, 0.0792, 0.173, 0, 0],'modified','qlim',[-140,140]/180*pi);
L(5)=Link([0, 0.0792, 0, pi/2, 0],'modified','qlim',[-140,140]/180*pi);
L(6)=Link([0, 0.225, 0, -pi/2, 0],'modified','qlim',[-360,360]/180*pi);
% 连杆偏置
L(2).offset=-pi/2;
L(4).offset=pi/2;
robot=SerialLink(L,'name','innfos_robot');
% robot.teach()
end
机械臂模型如图所示
step4.0 写机械臂辅助逆解程序
因为使用机械臂代数法逆解，所以需要计算矩阵，手写太麻烦并且容易出错，因此在matlab中定义各个关节的变换矩阵
% 机械臂逆解辅助程序
% by_xiAmAodou 2023-11-26
theta = sym('theta',[1 6]);
a_2 = sym('a_2');
a_3 = sym('a_3');
d = sym('d',[1 6]);
n = sym('n',[1 3]);
o = sym('o',[1 3]);
a = sym('a',[1 3]);
p = sym('p',[1 3]);
t01 = [ cos(theta(1)), -sin(theta(1)), 0,0;
sin(theta(1)), cos(theta(1)), 0,0;
0, 0, 1,d(1);
0, 0, 0,1];
t10 = [ cos(theta(1)), sin(theta(1)), 0,0;
-sin(theta(1)), cos(theta(1)), 0,0;
0, 0, 1,-d(1);
0, 0, 0,1];
t12 = [ sin(theta(2)), cos(theta(2)), 0,0;
0, 0, 1,d(2);
cos(theta(2)), -sin(theta(2)), 0,0;
0, 0, 0,1];
t23 = [ cos(theta(3)), -sin(theta(3)), 0,a_2;
sin(theta(3)), cos(theta(3)), 0,0;
0, 0, 1,d(3);
0, 0, 0,1];
t34 = [ -sin(theta(4)), -cos(theta(4)), 0,a_3;
cos(theta(4)), -sin(theta(4)), 0,0;
0, 0, 1,d(4);
0, 0, 0,1];
t45 = [ cos(theta(5)), -sin(theta(5)), 0,0;
0, 0, -1,-d(5);
sin(theta(5)), cos(theta(5)), 0,0;
0, 0, 0,1];
t54 = [ cos(theta(5)), 0, sin(theta(5)),0;
-sin(theta(5)), 0, cos(theta(5)),0;
0, -1, 0,-d(5);
0, 0, 0,1];
t56 = [ cos(theta(6)), -sin(theta(6)), 0,0;
0, 0, 1,d(6);
-sin(theta(6)), -cos(theta(6)), 0,0;
0, 0, 0,1];
t65 = [ cos(theta(6)), 0, -sin(theta(6)), 0;
-sin(theta(6)), 0, -cos(theta(6)), 0;
0, 1, 0, -d(6);
0, 0, 0, 1];
t =[n(1), o(1), a(1),p(1);
n(2), o(2), a(2),p(2);
n(3), o(3), a(3),p(3);
0, 0, 0, 1;];
lift_ = t10*t*t65;
right_ = t12*t23*t34*t45;
% 解theta 1 6 5
t10*t*t65
t12*t23*t34*t45
% 解theta 2 3 4
t10*t*t65*t54
t12*t23*t34
step5.0 解算 θ 1 \theta_1 θ1、 θ 5 \theta_5 θ5、 θ 6 \theta_6 θ6
step5.1 引入公式
设
m
cos
θ
−
n
sin
θ
=
d
(1)
m\cos\theta-n\sin\theta=d \tag {1}
mcosθ−nsinθ=d(1)
则设
m
=
ρ
sin
α
,
n
=
ρ
cos
α
(2)
m=\rho\sin\alpha,n=\rho\cos\alpha \tag {2}
m=ρsinα,n=ρcosα(2)
则
ρ
=
m
2
+
n
2
,
α
=
A
t
a
n
2
(
m
,
n
)
(3)
\rho=\sqrt{m^2+n^2},\alpha=Atan2(m,n) \tag {3}
ρ=m2+n2,α=Atan2(m,n)(3)
带入原方程得
sin
α
cos
θ
−
cos
α
sin
θ
=
d
ρ
(4)
\sin\alpha\cos\theta-\cos\alpha\sin\theta=\frac{d}{\rho} \tag {4}
sinαcosθ−cosαsinθ=ρd(4)
故
sin
(
α
−
θ
)
=
d
ρ
,
c
o
s
(
α
−
θ
)
=
±
1
−
d
2
ρ
2
(5)
\sin(\alpha-\theta)=\frac{d}{\rho},cos(\alpha-\theta)=\pm\sqrt{1-\frac{d^2}{\rho^2}} \tag {5}
sin(α−θ)=ρd,cos(α−θ)=±1−ρ2d2(5)
α
−
θ
=
A
t
a
n
2
(
d
ρ
,
±
1
−
d
2
ρ
2
)
(6)
\alpha-\theta=Atan2(\frac{d}{\rho},\pm\sqrt{1-\frac{d^2}{\rho^2}}) \tag {6}
α−θ=Atan2(ρd,±1−ρ2d2)(6)
θ
=
A
t
a
n
2
(
m
,
n
)
−
A
t
a
n
2
(
d
ρ
,
±
1
−
d
2
ρ
2
)
,
1
−
d
2
ρ
2
≥
0
(7)
\theta = Atan2(m,n)-Atan2(\frac{d}{\rho},\pm\sqrt{1-\frac{d^2}{\rho^2}}),\:\:1-\frac{d^2}{\rho^2}\geq 0 \tag {7}
θ=Atan2(m,n)−Atan2(ρd,±1−ρ2d2),1−ρ2d2≥0(7)
step5.2 观察
1
0
T
−
1
T
6
5
T
−
1
=
2
1
T
3
2
T
4
3
T
5
4
T
(8)
^0_1T^{-1}\ T\ ^5_6T^{-1}\ =^1_2T\ ^2_3T\ ^3_4T\ ^4_5T \tag {8}
10T−1 T 65T−1 =21T 32T 43T 54T(8)
KaTeX parse error: Unknown column alignment: 1 at position 22: … \begin{array}{1̲} \cos\theta_6(…
step5.3 解算 θ 1 \theta_1 θ1
由第二行第四列得到
p
y
cos
θ
1
−
d
6
(
a
y
cos
θ
1
−
a
x
sin
θ
1
)
−
p
x
sin
θ
1
=
d
2
+
d
3
+
d
4
(10)
p_y\cos\theta_1 - d_6(a_y\cos\theta_1 - a_x\sin\theta_1) - p_x\sin \theta_1=d_2+d_3+d_4 \tag {10}
pycosθ1−d6(aycosθ1−axsinθ1)−pxsinθ1=d2+d3+d4(10)
合并化简得到
(
p
y
−
d
6
a
y
)
cos
θ
1
−
(
d
6
a
x
+
p
x
)
sin
θ
1
=
d
2
+
d
3
+
d
4
(11)
(p_y - d_6a_y)\cos\theta_1 - (d_6a_x+ p_x)\sin \theta_1=d_2+d_3+d_4 \tag {11}
(py−d6ay)cosθ1−(d6ax+px)sinθ1=d2+d3+d4(11)
根据step5.1中的公式
设
m
1
=
p
y
−
d
6
a
y
(12)
m_1=p_y - d_6a_y \tag {12}
m1=py−d6ay(12)
n
1
=
d
6
a
x
+
p
x
(13)
n_1=d_6a_x+ p_x \tag {13}
n1=d6ax+px(13)
则有
θ
1
=
a
t
a
n
2
(
m
1
,
n
1
)
−
a
t
a
n
2
(
d
2
+
d
3
+
d
4
,
±
m
1
2
+
n
1
2
−
(
d
2
+
d
3
+
d
4
)
2
,
m
1
2
+
n
1
2
−
(
d
2
+
d
3
+
d
4
)
2
≥
0
(14)
\theta_1 = atan2(m_1,n_1)-atan2(d_2+d_3+d_4,\pm\sqrt{m_1^2 + n_1^2 - (d_2+d_3+d_4)^2},\:\:m_1^2 + n_1^2 - (d_2+d_3+d_4)^2\geq 0 \tag {14}
θ1=atan2(m1,n1)−atan2(d2+d3+d4,±m12+n12−(d2+d3+d4)2,m12+n12−(d2+d3+d4)2≥0(14)
解算出两个
θ
1
\theta_1
θ1的值
step5.4 解算 θ 5 \theta_5 θ5
由第二行第二列得到
cos
θ
5
=
a
y
cos
θ
1
−
a
x
sin
θ
1
(15)
\cos\theta_5=a_y\cos\theta_1 - a_x\sin\theta_1 \tag {15}
cosθ5=aycosθ1−axsinθ1(15)
故
θ
5
=
±
arccos
(
a
y
cos
θ
1
−
a
x
sin
θ
1
)
,
a
y
cos
θ
1
−
a
x
sin
θ
1
≤
1
(16)
\theta_5 = \pm\arccos(a_y\cos\theta_1 - a_x\sin\theta_1),\:\: a_y\cos\theta_1 - a_x\sin\theta_1 \leq 1 \tag {16}
θ5=±arccos(aycosθ1−axsinθ1),aycosθ1−axsinθ1≤1(16)
解算出四个
θ
5
\theta_5
θ5的值
step5.5 解算 θ 6 \theta_6 θ6
由第二行第一列，第二行第三列得到
cos
θ
6
(
n
y
cos
θ
1
−
n
x
sin
θ
1
)
−
s
i
n
θ
6
(
o
y
cos
θ
1
−
o
x
sin
θ
1
)
=
sin
θ
5
(17)
\cos\theta_6(n_y\cos\theta_1-n_x\sin\theta_1)-sin\theta_6(o_y\cos\theta_1-o_x\sin\theta_1)=\sin\theta_5 \tag {17}
cosθ6(nycosθ1−nxsinθ1)−sinθ6(oycosθ1−oxsinθ1)=sinθ5(17)
−
s
i
n
θ
6
(
n
y
cos
θ
1
−
n
x
sin
θ
1
)
−
cos
θ
6
(
o
y
cos
θ
1
−
o
x
sin
θ
1
)
=
0
(18)
-sin\theta_6(n_y\cos\theta_1-n_x\sin\theta_1)-\cos\theta_6(o_y\cos\theta_1-o_x\sin\theta_1)=0 \tag {18}
−sinθ6(nycosθ1−nxsinθ1)−cosθ6(oycosθ1−oxsinθ1)=0(18)
令
m
6
=
n
y
cos
θ
1
−
n
x
sin
θ
1
(19)
m_6 = n_y\cos\theta_1-n_x\sin\theta_1 \tag {19}
m6=nycosθ1−nxsinθ1(19)
n
6
=
o
y
cos
θ
1
−
o
x
sin
θ
1
(20)
n_6 = o_y\cos\theta_1-o_x\sin\theta_1 \tag {20}
n6=oycosθ1−oxsinθ1(20)
则
θ
6
=
a
t
a
n
2
(
−
n
6
sin
θ
5
,
m
6
sin
θ
5
)
,
sin
θ
5
≠
0
(21)
\theta_6 = atan2(\frac{-n_6}{\sin\theta_5},\frac{m_6}{\sin\theta_5}),\:\:\sin\theta_5 \neq 0 \tag {21}
θ6=atan2(sinθ5−n6,sinθ5m6),sinθ5=0(21)
解算出四个
θ
6
\theta_6
θ6的值
step6.0 解算 θ 2 \theta_2 θ2、 θ 3 \theta_3 θ3、 θ 4 \theta_4 θ4
由
1
0
T
−
1
T
6
5
T
−
1
5
4
T
−
1
=
2
1
T
3
2
T
4
3
T
(22)
^0_1T^{-1}\ T\ ^5_6T^{-1}\ ^4_5T^{-1}=^1_2T\ ^2_3T\ ^3_4T \tag {22}
10T−1 T 65T−1 54T−1=21T 32T 43T(22)
KaTeX parse error: Unknown column alignment: 1 at position 22: … \begin{array}{1̲} -\cos\theta_5…
step6.1 解算 θ 3 \theta_3 θ3
由第一行第四列和第三行第四列得
d
5
(
sin
θ
6
(
n
x
cos
θ
1
+
n
y
sin
θ
1
)
+
cos
θ
6
(
o
x
cos
θ
1
+
o
y
sin
θ
1
)
)
−
d
6
(
a
x
cos
θ
1
+
a
y
sin
θ
1
)
+
p
x
cos
θ
1
+
p
y
sin
θ
1
=
a
3
sin
(
θ
2
+
θ
3
)
+
a
2
sin
θ
2
(24)
d_5(\sin\theta_6(n_x\cos\theta_1+n_y\sin\theta_1)+\cos\theta_6(o_x\cos\theta_1+o_y\sin\theta_1))-d_6(a_x\cos\theta_1+a_y\sin\theta_1)+p_x\cos\theta_1+p_y\sin\theta_1=a_3\sin(\theta_2+\theta_3)+a_2\sin\theta_2 \tag {24}
d5(sinθ6(nxcosθ1+nysinθ1)+cosθ6(oxcosθ1+oysinθ1))−d6(axcosθ1+aysinθ1)+pxcosθ1+pysinθ1=a3sin(θ2+θ3)+a2sinθ2(24)
p
z
−
d
1
−
a
z
d
6
+
d
5
(
o
z
cos
θ
6
+
n
z
sin
θ
6
)
=
a
3
cos
(
θ
2
+
θ
3
)
+
a
2
cos
θ
2
(25)
p_z-d_1-a_zd_6+d_5(o_z\cos\theta_6+n_z\sin\theta_6)=a_3\cos(\theta_2+\theta_3)+a_2\cos\theta_2 \tag {25}
pz−d1−azd6+d5(ozcosθ6+nzsinθ6)=a3cos(θ2+θ3)+a2cosθ2(25)
令
m
3
=
d
5
(
sin
θ
6
(
n
x
cos
θ
1
+
n
y
sin
θ
1
)
+
cos
θ
6
(
o
x
cos
θ
1
+
o
y
sin
θ
1
)
)
−
d
6
(
a
x
cos
θ
1
+
a
y
sin
θ
1
)
+
p
x
cos
θ
1
+
p
y
sin
θ
1
(26)
m_3=d_5(\sin\theta_6(n_x\cos\theta_1+n_y\sin\theta_1)+\cos\theta_6(o_x\cos\theta_1+o_y\sin\theta_1))-d_6(a_x\cos\theta_1+a_y\sin\theta_1)+p_x\cos\theta_1+p_y\sin\theta_1 \tag {26}
m3=d5(sinθ6(nxcosθ1+nysinθ1)+cosθ6(oxcosθ1+oysinθ1))−d6(axcosθ1+aysinθ1)+pxcosθ1+pysinθ1(26)
n
3
=
p
z
−
d
1
−
a
z
d
6
+
d
5
(
o
z
cos
θ
6
+
n
z
sin
θ
6
)
(27)
n_3=p_z-d_1-a_zd_6+d_5(o_z\cos\theta_6+n_z\sin\theta_6) \tag {27}
n3=pz−d1−azd6+d5(ozcosθ6+nzsinθ6)(27)
则
θ
3
=
±
arccos
m
3
2
+
n
3
2
−
a
3
2
−
a
2
2
2
a
2
a
3
,
(
n
3
2
+
m
3
2
)
<
=
(
a
2
+
a
3
)
2
(28)
\theta_3 = \pm\arccos\frac{m_3^2+n_3^2-a_3^2-a_2^2}{2a_2a_3},\:\: (n_3^2 + m_3^2) <= (a_2+a_3)^2 \tag {28}
θ3=±arccos2a2a3m32+n32−a32−a22,(n32+m32)<=(a2+a3)2(28)
解算出八个
θ
3
\theta_3
θ3
step6.2 解算 θ 2 \theta_2 θ2
由公式24-27得
m
3
=
a
3
sin
(
θ
2
+
θ
3
)
+
a
2
sin
θ
2
(29)
m_3 = a_3\sin(\theta_2+\theta_3)+a_2\sin\theta_2 \tag {29}
m3=a3sin(θ2+θ3)+a2sinθ2(29)
n
3
=
a
3
cos
(
θ
2
+
θ
3
)
+
a
2
cos
θ
2
(30)
n_3 = a_3\cos(\theta_2+\theta_3)+a_2\cos\theta_2 \tag {30}
n3=a3cos(θ2+θ3)+a2cosθ2(30)
sin
θ
2
=
(
a
3
cos
θ
3
+
a
2
)
m
3
−
n
3
a
3
sin
θ
3
a
2
2
+
a
3
2
+
2
a
2
a
3
cos
θ
3
(31)
\sin\theta_2 = \frac{(a_3\cos\theta_3+a_2)m_3-n_3a_3\sin\theta_3}{a_2^2+a_3^2+2a_2a_3\cos\theta_3} \tag {31}
sinθ2=a22+a32+2a2a3cosθ3(a3cosθ3+a2)m3−n3a3sinθ3(31)
将
θ
3
\theta_3
θ3带入公式29、30得
cos
θ
2
=
n
3
+
a
3
sin
θ
3
sin
θ
2
a
3
cos
θ
3
+
a
2
\cos\theta_2 = \frac{n_3+a_3\sin\theta_3\sin\theta_2}{a_3\cos\theta_3+a_2}
cosθ2=a3cosθ3+a2n3+a3sinθ3sinθ2
θ
2
=
a
t
a
n
2
(
sin
θ
2
,
cos
θ
2
)
(32)
\theta_2 = atan2(\sin\theta_2,\cos\theta_2) \tag {32}
θ2=atan2(sinθ2,cosθ2)(32)
解算出八个
θ
2
\theta_2
θ2
step6.3 解算 θ 4 \theta_4 θ4
由公式23左右两边的第一行第二列与第三行第二列得
sin
θ
6
(
n
x
cos
θ
1
+
n
y
sin
θ
1
)
+
cos
θ
6
(
o
x
cos
θ
1
+
o
y
sin
θ
1
)
=
sin
(
θ
2
+
θ
3
+
θ
4
)
(33)
\sin\theta_6(n_x\cos\theta_1+n_y\sin\theta_1)+\cos\theta_6(o_x\cos\theta_1+o_y\sin\theta_1)=\sin(\theta_2+\theta_3+\theta_4) \tag {33}
sinθ6(nxcosθ1+nysinθ1)+cosθ6(oxcosθ1+oysinθ1)=sin(θ2+θ3+θ4)(33)
o
z
cos
θ
6
+
n
z
sin
θ
6
=
−
cos
(
θ
2
+
θ
3
+
θ
4
)
(34)
o_z\cos\theta_6+n_z\sin\theta_6=-\cos(\theta_2+\theta_3+\theta_4) \tag {34}
ozcosθ6+nzsinθ6=−cos(θ2+θ3+θ4)(34)
由公式33、34得
θ
4
=
a
t
a
n
2
(
−
sin
θ
6
(
n
x
cos
θ
1
+
n
y
sin
θ
1
)
−
cos
θ
6
(
o
x
cos
θ
1
+
o
y
sin
θ
1
)
,
−
o
z
cos
θ
6
−
n
z
sin
θ
6
)
−
θ
2
−
θ
3
(35)
\theta_4 = atan2(-\sin\theta_6(n_x\cos\theta_1 + n_y\sin\theta_1) - \cos\theta_6(o_x\cos\theta_1 + o_y\sin\theta_1),-o_z\cos\theta_6 - nz\sin\theta_6) - \theta_2 - \theta_3 \tag {35}
θ4=atan2(−sinθ6(nxcosθ1+nysinθ1)−cosθ6(oxcosθ1+oysinθ1),−ozcosθ6−nzsinθ6)−θ2−θ3(35)
解算出八个
θ
4
\theta_4
θ4
至此六个关节角全部解算完毕
验证：
%% 新建机械臂
innfos_robot = creat_robot_arm();
a = innfos_robot.a;
d = innfos_robot.d;
q = randi([-2,2],1,6);
t = innfos_robot.fkine(q);
t
theta = gluon_ik_8(a,d,t);
theta
for i = 1:size(theta,1)
t = innfos_robot.fkine(theta(i,:));
t
end
输出：
t =
0.6291 0.3919 -0.6713 -0.03179
-0.2794 0.9199 0.2753 0.0228
0.7254 0.0144 0.6882 0.3056
0 0 0 1
theta =
-1.0000 0.1761 2.9243 -2.2420 2.0000 -1.1416
-1.0000 3.1005 -2.9243 0.6823 2.0000 -1.1416
-1.0000 -0.0000 2.0000 -4.2832 -2.0000 2.0000
-1.0000 2.0000 -2.0000 -2.2832 -2.0000 2.0000
-2.7759 -2.0222 1.9961 2.2518 2.0911 0.9765
-2.7759 -0.0261 -1.9961 4.2479 2.0911 0.9765
-2.7759 -3.0126 2.9414 -0.8448 -2.0911 -2.1651
-2.7759 -0.0712 -2.9414 2.0967 -2.0911 -2.1651
t =
0.6291 0.3919 -0.6713 -0.03179
-0.2794 0.9199 0.2753 0.0228
0.7254 0.0144 0.6882 0.3056
0 0 0 1
t =
0.6291 0.3919 -0.6713 -0.03179
-0.2794 0.9199 0.2753 0.0228
0.7254 0.0144 0.6882 0.3056
0 0 0 1
t =
0.6291 0.3919 -0.6713 -0.03179
-0.2794 0.9199 0.2753 0.0228
0.7254 0.0144 0.6882 0.3056
0 0 0 1
t =
0.6291 0.3919 -0.6713 -0.03179
-0.2794 0.9199 0.2753 0.0228
0.7254 0.0144 0.6882 0.3056
0 0 0 1
t =
0.6291 0.3919 -0.6713 -0.03179
-0.2794 0.9199 0.2753 0.0228
0.7254 0.0144 0.6882 0.3056
0 0 0 1
t =
0.6291 0.3919 -0.6713 -0.03179
-0.2794 0.9199 0.2753 0.0228
0.7254 0.0144 0.6882 0.3056
0 0 0 1
t =
0.6291 0.3919 -0.6713 -0.03179
-0.2794 0.9199 0.2753 0.0228
0.7254 0.0144 0.6882 0.3056
0 0 0 1
t =
0.6291 0.3919 -0.6713 -0.03179
-0.2794 0.9199 0.2753 0.0228
0.7254 0.0144 0.6882 0.3056
0 0 0 1
扫一扫
2701
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
