Exp.1:在三连杆平面机械臂中， X ^ 3 \widehat{\boldsymbol{X}}_{3} X 3 和 X ^ 0 \widehat{\boldsymbol{X}}_{0} X 0之间的夹角为 φ \varphi φ，关节3的位置为 [ x y 0 ] T \left[\begin{array}{lll}x & y & 0\end{array}\right]^{T} [xy0]T，连杆1和连杆2的长度分别为 L 1 L_{1} L1 和 L 2 L_{2} L2。找出 θ 1 , θ 2 \theta_{1}, \theta_{2} θ1,θ2和 θ 3 \theta_{3} θ3。
由末端执行器的位置和姿态表示的变换矩阵为:
3
0
T
=
[
c
φ
−
s
φ
0
x
s
φ
c
φ
0
y
0
0
1
0
0
0
0
1
]
{ }_{3}^{0}{T}=\left[\begin{array}{cccc} {c}_{\varphi} & -s_{\varphi} & 0 & x \\ s_{\varphi} & c_{\varphi} & 0 & y \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]
30T=⎣⎢⎢⎡cφsφ00−sφcφ000010xy01⎦⎥⎥⎤
| D-H 参数 | α i − 1 \alpha_{i-1} αi−1 | a i − 1 a_{i-1} ai−1 | d i d_{i} di | θ i \theta_{i} θi |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | θ 1 \theta_{1} θ1 |
| 2 | 0 0 0 | L 1 L_{1} L1 | 0 0 0 | θ 2 \theta_{2} θ2 |
| 3 | 0 0 0 | L 2 L_{2} L2 | 0 0 0 | θ 3 \theta_{3} θ3 |
关节变量表示的变换矩阵:
3
0
T
=
[
c
123
−
s
123
0
L
1
c
1
+
L
2
c
12
s
123
c
123
0
L
1
s
1
+
L
2
s
12
0
0
1
0
0
0
0
1
]
{ }_{3}^{0} T=\left[\begin{array}{cccc} c_{123} & -s_{123} & 0 & L_{1} c_{1}+L_{2} c_{12} \\ s_{123} & c_{123} & 0 & L_{1} s_{1}+L_{2} s_{12} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]
30T=⎣⎢⎢⎡c123s12300−s123c123000010L1c1+L2c12L1s1+L2s1201⎦⎥⎥⎤
c 123 = cos ( θ 1 + θ 2 + θ 3 ) s 123 = sin ( θ 1 + θ 2 + θ 3 ) \begin{aligned} &c_{123}=\cos \left(\theta_{1}+\theta_{2}+\theta_{3}\right) \\ &s_{123}=\sin \left(\theta_{1}+\theta_{2}+\theta_{3}\right) \end{aligned} c123=cos(θ1+θ2+θ3)s123=sin(θ1+θ2+θ3)
3 0 T = [ c φ − s φ 0 x s φ c φ 0 y 0 0 1 0 0 0 0 1 ] 3 0 T = [ c 123 − s 123 0 L 1 c 1 + L 2 c 12 s 123 c 123 0 L 1 s 1 + L 2 s 12 0 0 1 0 0 0 0 1 ] { }_{3}^{0} T=\left[\begin{array}{cccc} c_{\varphi} & -s_{\varphi} & 0 & x \\ s_{\varphi} & c_{\varphi} & 0 & y \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] \quad{ }_{3}^{0} T=\left[\begin{array}{cccc} c_{123} & -s_{123} & 0 & L_{1} c_{1}+L_{2} c_{12} \\ s_{123} & c_{123} & 0 & L_{1} s_{1}+L_{2} s_{12} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] 30T=⎣⎢⎢⎡cφsφ00−sφcφ000010xy01⎦⎥⎥⎤30T=⎣⎢⎢⎡c123s12300−s123c123000010L1c1+L2c12L1s1+L2s1201⎦⎥⎥⎤
比较两个矩阵，我们可以得到：
c
φ
=
c
123
s
φ
=
s
123
x
=
L
1
c
1
+
L
2
c
12
y
=
L
1
s
1
+
L
2
s
12
\begin{aligned} &c_{\varphi}=c_{123}\\ &s_{\varphi}=s_{123}\\ &x=L_{1} c_{1}+L_{2} c_{12}\\ &y=L_{1} s_{1}+L_{2} s_{12} \end{aligned}
cφ=c123sφ=s123x=L1c1+L2c12y=L1s1+L2s12
通过计算
x
2
+
y
2
x^2+y^2
x2+y2，可得:
x
2
+
y
2
=
L
1
2
+
L
1
2
+
2
L
1
L
2
c
2
x^{2}+y^{2}=L_{1}^{2}+L_{1}^{2}+2 L_{1} L_{2} c_{2}
x2+y2=L12+L12+2L1L2c2
因此
c
2
=
(
x
2
+
y
2
−
L
1
2
−
L
1
2
)
/
(
2
L
1
L
2
)
c_{2}=\left(x^{2}+y^{2}-L_{1}^{2}-L_{1}^{2}\right) /\left(2 L_{1} L_{2}\right)
c2=(x2+y2−L12−L12)/(2L1L2)
如果
∣
x
2
+
y
2
−
L
1
2
−
L
1
2
2
L
1
L
2
∣
>
1
\left|\frac{x^{2}+y^{2}-L_{1}^{2}-L_{1}^{2}}{2 L_{1} L_{2}}\right|>1
∣∣∣2L1L2x2+y2−L12−L12∣∣∣>1，那么这个问题无解；
如果 ∣ x 2 + y 2 − L 1 2 − L 1 2 2 L 1 L 2 ∣ < 1 \left|\frac{x^{2}+y^{2}-L_{1}^{2}-L_{1}^{2}}{2 L_{1} L_{2}}\right|<1 ∣∣∣2L1L2x2+y2−L12−L12∣∣∣<1，
c
2
=
(
x
2
+
y
2
−
L
1
2
−
L
1
2
)
/
(
2
L
1
L
2
)
θ
2
=
±
arccos
[
x
2
+
y
2
−
L
1
2
−
L
1
2
2
L
1
L
2
]
\begin{aligned} &c_{2}=\left(x^{2}+y^{2}-L_{1}^{2}-L_{1}^{2}\right) /\left(2 L_{1} L_{2}\right) \\ &\theta_{2}=\pm \arccos \left[\frac{x^{2}+y^{2}-L_{1}^{2}-L_{1}^{2}}{2 L_{1} L_{2}}\right] \end{aligned}
c2=(x2+y2−L12−L12)/(2L1L2)θ2=±arccos[2L1L2x2+y2−L12−L12]
重写有关
x
,
y
x,y
x,y的等式
x
=
L
1
c
1
+
L
2
(
c
1
c
2
−
s
1
s
2
)
y
=
L
1
s
1
+
L
2
(
c
1
s
2
+
s
1
c
2
)
\begin{aligned} &x=L_{1} c_{1}+L_{2}\left(c_{1} c_{2}-s_{1} s_{2}\right) \\ &y=L_{1} s_{1}+L_{2}\left(c_{1} s_{2}+s_{1} c_{2}\right) \end{aligned}
x=L1c1+L2(c1c2−s1s2)y=L1s1+L2(c1s2+s1c2)
我们将
cos
θ
1
\cos \theta_{1}
cosθ1和
s
i
n
θ
1
\\sin \theta_{1}
sinθ1分别提取出来，得到以下式子
x
=
(
L
1
+
L
2
c
2
)
c
1
−
(
L
2
s
2
)
s
1
y
=
(
L
1
+
L
2
c
2
)
s
1
+
(
L
2
s
2
)
c
1
\begin{aligned} &x=\left(L_{1}+L_{2} c_{2}\right) c_{1}-\left(L_{2} s_{2}\right) s_{1} \\ &y=\left(L_{1}+L_{2} c_{2}\right) s_{1}+\left(L_{2} s_{2}\right) c_{1} \end{aligned}
x=(L1+L2c2)c1−(L2s2)s1y=(L1+L2c2)s1+(L2s2)c1
换元
x
=
k
1
c
1
−
k
2
s
1
y
=
k
1
s
1
+
k
2
c
1
\begin{aligned} &x=k_{1} c_{1}-k_{2} s_{1} \\ &y=k_{1} s_{1}+k_{2} c_{1} \end{aligned}
x=k1c1−k2s1y=k1s1+k2c1
k 1 = L 1 + L 2 c 2 k 2 = L 2 s 2 \begin{aligned} &k_{1}=L_{1}+L_{2} c_{2} \\ &k_{2}=L_{2} s_{2} \end{aligned} k1=L1+L2c2k2=L2s2
令 r = k 1 2 + k 2 2 r=\sqrt{k_{1}^{2}+k_{2}^{2}} r=k12+k22， γ = arctan ( k 2 k 1 ) \gamma=\arctan \left(\frac{k_{2}}{k_{1}}\right) γ=arctan(k1k2)
因此
k
1
=
r
cos
γ
k
2
=
r
sin
γ
\begin{aligned} &k_{1}=r \cos \gamma \\ &k_{2}=r \sin \gamma \end{aligned}
k1=rcosγk2=rsinγ
θ 1 = arctan ( y x ) − arctan ( k 2 k 1 ) \theta_{1}=\arctan \left(\frac{y}{x}\right)-\arctan \left(\frac{k_{2}}{k_{1}}\right) θ1=arctan(xy)−arctan(k1k2)
将
k
1
k_{1}
k1和
k
2
k_{2}
k2的值分别代入到原来有关
x
,
y
x,y
x,y的式子中，我们可以得到
γ
+
θ
1
=
arctan
(
y
x
)
\gamma+\theta_{1}=\arctan \left(\frac{y}{x}\right)
γ+θ1=arctan(xy)
所以
θ
1
=
arctan
(
y
x
)
−
arctan
(
k
2
k
1
)
\theta_{1}=\arctan \left(\frac{y}{x}\right)-\arctan \left(\frac{k_{2}}{k_{1}}\right)
θ1=arctan(xy)−arctan(k1k2)
又因为
θ
1
+
θ
2
+
θ
3
=
arctan
(
s
φ
c
φ
)
=
φ
\theta_{1}+\theta_{2}+\theta_{3}=\arctan \left(\frac{s_{\varphi}}{c_{\varphi}}\right)=\varphi
θ1+θ2+θ3=arctan(cφsφ)=φ
θ
3
=
φ
−
θ
1
−
θ
2
\theta_{3}=\varphi-\theta_{1}-\theta_{2}
θ3=φ−θ1−θ2
出自：机器人学入门手册
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
