lab4 机械臂逆运动学求解
逆运动学解析解
我们已知的是末端的位姿\(T\)，满足：
\[
T = T_0^6 = T_1^0T_2^1T_3^2T_4^3T_5^4T_6^5
\]
我们假设
\[
T =
\left(\begin{array}{cccc} \mathrm{nx} & \mathrm{ox} & \mathrm{ax} & \mathrm{dx}\\ \mathrm{ny} & \mathrm{oy} & \mathrm{ay} & \mathrm{dy}\\ \mathrm{nz} & \mathrm{oz} & \mathrm{az} & \mathrm{dz}\\ 0 & 0 & 0 & 1 \end{array}\right)
\]
其中，\(T_0^6\)是末端的全局位姿，\(T_i^{i-1}\)是第\(i\)个关节的变换矩阵。
通过这里，可以算出\(T_6^1\)满足：
\[
T_6^1 = T_2^1T_3^2T_4^3T_5^4T_6^5
\]
代入上面的变换矩阵，可以得到：
\[
T_6^1 =
\left(\begin{array}{cccc} \sin\left(t_{2}+t_{3}+t_{4}\right)\,\sin\left(t_{6}\right)-\cos\left(t_{2}+t_{3}+t_{4}\right)\,\cos\left(t_{6}\right)\,\sin\left(t_{5}\right) & \sin\left(t_{2}+t_{3}+t_{4}\right)\,\cos\left(t_{6}\right)+\cos\left(t_{2}+t_{3}+t_{4}\right)\,\sin\left(t_{5}\right)\,\sin\left(t_{6}\right) & \cos\left(t_{2}+t_{3}+t_{4}\right)\,\cos\left(t_{5}\right) & \frac{17\,\sin\left(t_{2}+t_{3}\right)}{100}+\frac{37\,\sin\left(t_{2}\right)}{200}-\cos\left(t_{5}\right)\,\left(\frac{171\,\sin\left(t_{2}+t_{3}\right)\,\sin\left(t_{4}\right)}{2000}-\frac{171\,\cos\left(t_{2}+t_{3}\right)\,\cos\left(t_{4}\right)}{2000}\right)+\frac{77\,\cos\left(t_{2}+t_{3}\right)\,\sin\left(t_{4}\right)}{1000}+\frac{77\,\sin\left(t_{2}+t_{3}\right)\,\cos\left(t_{4}\right)}{1000}\\ -\cos\left(t_{2}+t_{3}+t_{4}\right)\,\sin\left(t_{6}\right)-\sin\left(t_{2}+t_{3}+t_{4}\right)\,\cos\left(t_{6}\right)\,\sin\left(t_{5}\right) & \sin\left(t_{2}+t_{3}+t_{4}\right)\,\sin\left(t_{5}\right)\,\sin\left(t_{6}\right)-\cos\left(t_{2}+t_{3}+t_{4}\right)\,\cos\left(t_{6}\right) & \sin\left(t_{2}+t_{3}+t_{4}\right)\,\cos\left(t_{5}\right) & \frac{77\,\sin\left(t_{2}+t_{3}\right)\,\sin\left(t_{4}\right)}{1000}-\frac{37\,\cos\left(t_{2}\right)}{200}-\frac{17\,\cos\left(t_{2}+t_{3}\right)}{100}+\cos\left(t_{5}\right)\,\left(\frac{171\,\cos\left(t_{2}+t_{3}\right)\,\sin\left(t_{4}\right)}{2000}+\frac{171\,\sin\left(t_{2}+t_{3}\right)\,\cos\left(t_{4}\right)}{2000}\right)-\frac{77\,\cos\left(t_{2}+t_{3}\right)\,\cos\left(t_{4}\right)}{1000}\\ \cos\left(t_{5}\right)\,\cos\left(t_{6}\right) & -\cos\left(t_{5}\right)\,\sin\left(t_{6}\right) & \sin\left(t_{5}\right) & \frac{171\,\sin\left(t_{5}\right)}{2000}+\frac{23}{1000}\\ 0 & 0 & 0 & 1 \end{array}\right)
\]
而这个结果等于：
\[
(T_1^0)^{-1}T = T_6^1
\]
因为T是已知的，我们可以计算出等式的左边：
\[
(T_1^0)^{-1}T =
\left(\begin{array}{cccc} \mathrm{nx}\,\cos\left(t_{1}\right)+\mathrm{ny}\,\sin\left(t_{1}\right) & \mathrm{ox}\,\cos\left(t_{1}\right)+\mathrm{oy}\,\sin\left(t_{1}\right) & \mathrm{ax}\,\cos\left(t_{1}\right)+\mathrm{ay}\,\sin\left(t_{1}\right) & \mathrm{dx}\,\cos\left(t_{1}\right)+\mathrm{dy}\,\sin\left(t_{1}\right)\\ -\mathrm{nz} & -\mathrm{oz} & -\mathrm{az} & \frac{23}{100}-\mathrm{dz}\\ \mathrm{ny}\,\cos\left(t_{1}\right)-\mathrm{nx}\,\sin\left(t_{1}\right) & \mathrm{oy}\,\cos\left(t_{1}\right)-\mathrm{ox}\,\sin\left(t_{1}\right) & \mathrm{ay}\,\cos\left(t_{1}\right)-\mathrm{ax}\,\sin\left(t_{1}\right) & \mathrm{dy}\,\cos\left(t_{1}\right)-\mathrm{dx}\,\sin\left(t_{1}\right)\\ 0 & 0 & 0 & 1 \end{array}\right)
\]
对比这两个矩阵的不同形式，注意到这两个矩阵第三行的后面两项，即
\[
\left\{
\begin{align*}
\mathrm{ay}\,\cos\left(t_{1}\right)-\mathrm{ax}\,\sin\left(t_{1}\right)
&= \sin\left(t_{5}\right)\\
\mathrm{dy}\,\cos\left(t_{1}\right)-\mathrm{dx}\,\sin\left(t_{1}\right)
&= \frac{171\,\sin\left(t_{5}\right)}{2000}+\frac{23}{1000}
\end{align*}
\right.
\]
根据这个方程组，消去\(\sin\left(t_{5}\right)\)，可以得到关于\(a_1\)的方程：
\[
\left(\mathrm{dy} - 0.0855 \mathrm{ay}\right)\cos\left(t_{1}\right) + \left(0.0855 \mathrm{ax} - \mathrm{dx}\right)\sin\left(a_{1}\right) = 0.023
\]
从这个方程可以得到
\[
t_1 = \arctan2\left(d_y, d_x\right) - \arctan2\left(d_2, \pm \sqrt{d_x^2+d_y^2-d_2^2} \right)
\]
其中
\[
\left\{
\begin{align*}
d_x &= \mathrm{dx} - 0.0855 \mathrm{ax}\\
d_y &= \mathrm{dy} - 0.0855 \mathrm{ay}\\
d_2 &= 0.023
\end{align*}
\right.
\]
这样，我们就可以得到\(t_1\)的值。然后，我们可以得到\(t_5\)的值：
\[
t_5 = \arcsin\left(\mathrm{ay}\,\cos\left(t_{1}\right)-\mathrm{ax}\,\sin\left(t_{1}\right) \right) （\mathrm{ay}\,\cos\left(t_{1}\right)-\mathrm{ax}\,\sin\left(t_{1}\right) \leq 1)
\]
得到了\(t_1\)和\(t_5\)的值之后，我们可以得到\(t_6\)的值，\(t_6\)满足：
\[
\left\{
\begin{align*}
\cos\left(t_{5}\right)\,\cos\left(t_{6}\right) &= \mathrm{ny}\,\cos\left(t_{1}\right)-\mathrm{nx}\,\sin\left(t_{1}\right)\\
-\cos\left(t_{5}\right)\,\sin\left(t_{6}\right) &= \mathrm{oy}\,\cos\left(t_{1}\right)-\mathrm{ox}\,\sin\left(t_{1}\right)
\end{align*}
\right.
\]
所以，
\[
t_6 = \arctan2\left(\mathrm{ox}\,\sin\left(t_{1}\right)-\mathrm{oy}\,\cos\left(t_{1}\right), \mathrm{ny}\,\cos\left(t_{1}\right)-\mathrm{nx}\,\sin\left(t_{1}\right)\right)
\]
接下来，让我们一鼓作气，求出剩下的值。因为\(t_1\)，\(t_5\)，\(t_6\)已经求出，我们可以得到：
\[
T_4^1 = T_2^1T_3^2T_4^3
\]
带入变换矩阵，可以得到：
\[
T_4^1 =
\left(\begin{array}{cccc} \cos\left(t_{2}+t_{3}+t_{4}\right) & 0 & \sin\left(t_{2}+t_{3}+t_{4}\right) & \frac{17\,\sin\left(t_{2}+t_{3}\right)}{100}+\frac{37\,\sin\left(t_{2}\right)}{200}\\ \sin\left(t_{2}+t_{3}+t_{4}\right) & 0 & -\cos\left(t_{2}+t_{3}+t_{4}\right) & -\frac{17\,\cos\left(t_{2}+t_{3}\right)}{100}-\frac{37\,\cos\left(t_{2}\right)}{200}\\ 0 & 1 & 0 & \frac{23}{1000}\\ 0 & 0 & 0 & 1 \end{array}\right)
\]
而和上面类似，这个式子还等于：
\[
(T_1^0)^{-1}T(T_6^5)^{-1}(T_5^4)^{-1} = T_4^1
\]
这个\(T_4^1\)中的每一个值我们都是已知的，十分复杂，这里不列出它的具体形式，所以只知道这个值能算就行了。
这个东西能算，我们观察第一个\(T_4^1\)公式的第一行的第四列和第二行的第四列，感觉好像能解。
所以就代入过来：
\[
\left\{
\begin{align*}
\frac{17\,\sin\left(t_{2}+t_{3}\right)}{100}+\frac{37\,\sin\left(t_{2}\right)}{200}
&= \mathrm{dx}\,\cos\left(t_{1}\right)-\frac{171\,\mathrm{ax}\,\cos\left(t_{1}\right)}{2000}-\frac{171\,\mathrm{ay}\,\sin\left(t_{1}\right)}{2000}+\mathrm{dy}\,\sin\left(t_{1}\right)-\frac{77\,\mathrm{ox}\,\cos\left(t_{1}\right)\,\cos\left(t_{6}\right)}{1000}-\frac{77\,\mathrm{nx}\,\cos\left(t_{1}\right)\,\sin\left(t_{6}\right)}{1000}-\frac{77\,\mathrm{oy}\,\cos\left(t_{6}\right)\,\sin\left(t_{1}\right)}{1000}-\frac{77\,\mathrm{ny}\,\sin\left(t_{1}\right)\,\sin\left(t_{6}\right)}{1000}\\
-\frac{17\,\cos\left(t_{2}+t_{3}\right)}{100}-\frac{37\,\cos\left(t_{2}\right)}{200}
&= \frac{171\,\mathrm{az}}{2000}-\mathrm{dz}+\frac{77\,\mathrm{oz}\,\cos\left(t_{6}\right)}{1000}+\frac{77\,\mathrm{nz}\,\sin\left(t_{6}\right)}{1000}+\frac{23}{100}
\end{align*}
\right.
\]
这个式子右边的项来自那个非常非常复杂的式子，但好在通过前面的分析我们是知道它的值的。
注意到，式子左边平方相加之后的结果是：
\[
{C_{1}}^2+2\,\cos\left(t_{3}\right)\,C_{1}\,C_{2}+{C_{2}}^2
\]
所以我们在这里就能求出\(t_3\)的值，即
\[
t_3 = \pm \arccos\left(\frac{A_1^2 + A_2^2 - C_1^2 - C_2^2}{2\,C_{1}\,C_{2}}\right)
\]
其中
\[
\left\{
\begin{align*}
A_1 &=
\mathrm{dx}\,\cos\left(t_{1}\right)-\frac{171\,\mathrm{ax}\,\cos\left(t_{1}\right)}{2000}-\frac{171\,\mathrm{ay}\,\sin\left(t_{1}\right)}{2000}+\mathrm{dy}\,\sin\left(t_{1}\right)-\frac{77\,\mathrm{ox}\,\cos\left(t_{1}\right)\,\cos\left(t_{6}\right)}{1000}-\frac{77\,\mathrm{nx}\,\cos\left(t_{1}\right)\,\sin\left(t_{6}\right)}{1000}-\frac{77\,\mathrm{oy}\,\cos\left(t_{6}\right)\,\sin\left(t_{1}\right)}{1000}-\frac{77\,\mathrm{ny}\,\sin\left(t_{1}\right)\,\sin\left(t_{6}\right)}{1000}\\
A_2 &= -\frac{171\,\mathrm{az}}{2000}+\mathrm{dz}-\frac{77\,\mathrm{oz}\,\cos\left(t_{6}\right)}{1000}-\frac{77\,\mathrm{nz}\,\sin\left(t_{6}\right)}{1000}-\frac{23}{100}
\end{align*}
\right.
\]
这样，我们就可以得到\(t_3\)的值。接下来，我们可以得到\(t_2\)的值，这个值从上面那个矩阵的第一行的第三列就能的出来.。
\[
\left(C_1 \cos(t_3) + C_2 \right) \sin(t_2) + C_1 \sin(t_3) \cos(t_2) = A_1
\]
解得
\[
t_2 = \arctan2\left(d_y, d_x\right) - \arctan2\left(d_2, \pm \sqrt{d_x^2+d_y^2-d_2^2} \right)
\]
其中
\[
\left\{
\begin{align*}
d_x &= -\left(C_1 \cos(t_3) + C_2 \right)\\
d_y &= C_1 \sin(t_3)\\
d_2 &= A_1\\
C_1&=0.170, C_2=0.185
\end{align*}
\right.
\]
这样，我们就可以得到\(t_2\)的值。最后，我们可以得到\(t_4\)的值，这个值从上面那个\(T_4^1\)的第一行第一列和第二行第二列的比值就能求出来，显而易见：
\[
\left\{
\begin{align*}
\cos\left(t_2+t_3+t_4\right) &= \mathrm{ax}\,\cos\left(t_{1}\right)\,\cos\left(t_{5}\right)+\mathrm{ay}\,\cos\left(t_{5}\right)\,\sin\left(t_{1}\right)-\mathrm{nx}\,\cos\left(t_{1}\right)\,\cos\left(t_{6}\right)\,\sin\left(t_{5}\right)-\mathrm{ny}\,\cos\left(t_{6}\right)\,\sin\left(t_{1}\right)\,\sin\left(t_{5}\right)+\mathrm{ox}\,\cos\left(t_{1}\right)\,\sin\left(t_{5}\right)\,\sin\left(t_{6}\right)+\mathrm{oy}\,\sin\left(t_{1}\right)\,\sin\left(t_{5}\right)\,\sin\left(t_{6}\right)\\
\sin\left(t_2+t_3+t_4\right) &= \mathrm{nz}\,\cos\left(t_{6}\right)\,\sin\left(t_{5}\right)-\mathrm{az}\,\cos\left(t_{5}\right)-\mathrm{oz}\,\sin\left(t_{5}\right)\,\sin\left(t_{6}\right)
\end{align*}
\right.
\]
\[
t_4 = \arctan2\left(\sin\left(t_2+t_3+t_4\right), \cos\left(t_2+t_3+t_4\right)\right) - t_2 - t_3
\]
至此，我们已经求出了逆运动学的解析解。
源代码
import numpy as np
from math import cos, sin, atan2, sqrt, asin, acos, pi
class solution:
def __init__(self, theta):
locations = theta[:3]
thetas = theta[3:]
t1, t2, t3 = thetas[0], thetas[1], thetas[2]
self.T = np.zeros((4,4))
self.T[0, 3] = locations[0]
self.T[1, 3] = locations[1]
self.T[2, 3] = locations[2]
self.T[0, 0] = cos(t2) * cos(t3)
self.T[0, 1] = -cos(t2) * sin(t3)
self.T[0, 2] = sin(t2)
self.T[1, 0] = sin(t1) * sin(t2) * cos(t3) + cos(t1) * sin(t3)
self.T[1, 1] = -sin(t1) * sin(t2) * sin(t3) + cos(t1) * cos(t3)
self.T[1, 2] = -sin(t1) * cos(t2)
self.T[2, 0] = -cos(t1) * sin(t2) * cos(t3) + sin(t1) * sin(t3)
self.T[2, 1] = cos(t1) * sin(t2) * sin(t3) + sin(t1) * cos(t3)
self.T[2, 2] = cos(t1) * cos(t2)
self.nx, self.ny, self.nz = self.T[0, 0], self.T[1, 0], self.T[2, 0]
self.ox, self.oy, self.oz = self.T[0, 1], self.T[1, 1], self.T[2, 1]
self.ax, self.ay, self.az = self.T[0, 2], self.T[1, 2], self.T[2, 2]
self.dx, self.dy, self.dz = self.T[0, 3], self.T[1, 3], self.T[2, 3]
def IK(self):
t1 = self._cal_t1()
t5 = self._cal_t5(t1=t1)
t6 = self._cal_t6(t1=t1)
t3 = self._cal_t3(t1=t1, t6=t6)
t2 = self._cal_t2(t1=t1, t3=t3, t6=t6)
t4 = self._cal_t4(t1=t1, t2=t2, t3=t3, t5=t5, t6=t6)
return [t1, t2, t3, t4 , t5, t6]
def _cal_t1(self):
dx = self.dx - 0.0855 * self.ax
dy = self.dy - 0.0855 * self.ay
d2 = 0.023
dt = sqrt(dx * dx + dy * dy - d2 * d2)
t = np.zeros(2)
t[0] = atan2(dy, dx) - atan2(d2, dt)
t[1] = atan2(dy, dx) - atan2(d2, -dt)
return t[0]
def _cal_t5(self, t1):
ay = self.ay
ax = self.ax
return asin(ay * cos(t1) - ax * sin(t1))
def _cal_t6(self, t1):
ox = self.ox
oy = self.oy
nx = self.nx
ny = self.ny
return atan2(ox * sin(t1) - oy * cos(t1), ny * cos(t1) - nx * sin(t1))
def _cal_t3(self, t1, t6):
dx = self.dx
dy = self.dy
ax = self.ax
ay = self.ay
ox = self.ox
nx = self.nx
oy = self.oy
ny = self.ny
az = self.az
dz = self.dz
oz = self.oz
nz = self.nz
C1 = 17/100
C2 = 37/200
A1 = (dx * cos(t1) - 0.0855 * ax * cos(t1) - 0.0855 * ay * sin(t1)
+ dy * sin(t1)
- 0.077 * ox * cos(t1) * cos(t6)
- 0.077 * nx * cos(t1) * sin(t6)
- 0.077 * oy * sin(t1) * cos(t6)
- 0.077 * ny * sin(t1) * sin(t6)
)
A2 = (-171 * az /2000 + dz - 77 * oz * cos(t6) / 1000
- 77 * nz * sin(t6) / 1000 - 0.23)
t3 = -acos((A1 * A1 + A2 * A2 - C1 * C1 - C2 * C2) / (2 * C1 * C2))
return t3
def _cal_t2(self, t1, t3, t6):
dx = self.dx
dy = self.dy
ax = self.ax
ay = self.ay
ox = self.ox
nx = self.nx
oy = self.oy
ny = self.ny
A1 = (dx * cos(t1) - 171 * ax * cos(t1) / 2000 - 171 * ay * sin(t1) /2000
+ dy * sin(t1)
- 77 * ox * cos(t1) * cos(t6) / 1000
- 77 * nx * cos(t1) * sin(t6) / 1000
- 77 * oy * sin(t1) * cos(t6) / 1000
- 77 * ny * sin(t1) * sin(t6) / 1000
)
C1 = 17/100
C2 = 37/200
dx = -(C1 * cos(t3) + C2)
dy = C1 * sin(t3)
d2 = A1
dt = sqrt(dx * dx + dy * dy - d2 * d2)
t = np.zeros(2)
t[0] = atan2(dy, dx) - atan2(d2, dt)
t[1] = atan2(dy, dx) - atan2(d2, -dt)
return t[0]
def _cal_t4(self, t1, t5, t2, t3, t6):
ax = self.ax
ay = self.ay
az = self.az
nx = self.nx
ny = self.ny
ox = self.ox
oy = self.oy
nz = self.nz
oz = self.oz
c234 = (ax * cos(t1) * cos(t5) +
ay * cos(t5) * sin(t1) -
nx * cos(t1) * sin(t5) * cos(t6) -
ny * cos(t6) * sin(t1) * sin(t5) +
ox * cos(t1) * sin(t5) * sin(t6) +
oy * sin(t1) * sin(t5) * sin(t6))
s234 = (nz * cos(t6) * sin(t5) - az * cos(t5) -
oz * sin(t5) * sin(t6))
t4 = atan2(s234, c234) - t2 - t3
return t4
if __name__ == '__main__':
end_effector_pose = [[0.117, 0.334, 0.499, -2.019, -0.058, -2.190],
[-0.066, 0.339, 0.444, -2.618, -0.524, -3.141],
[0.3, 0.25, 0.26, -2.64, 0.59, -2.35],
[0.42, 0, 0.36, 3.14, 1, -1.57],
[0.32, -0.25, 0.16, 3, 0.265, -0.84]]
Solver = solution(end_effector_pose[4])
result = Solver.IK()
print(result)
验证结果
要求代入如下几组的数据实验并记录结果：
| 序号 | \(x\) | \(y\) | \(z\) | \(\phi\) | \(\theta\) | \(\psi\) |
|---|---|---|---|---|---|---|
| 1 | 0.117 | 0.334 | 0.499 | -2.019 | -0.058 | -2.190 |
| 2 | -0.066 | 0.339 | 0.444 | -2.618 | -0.524 | -3.141 |
| 3 | 0.3 | 0.25 | 0.26 | -2.64 | 0.59 | -2.35 |
| 4 | 0.42 | 0 | 0.36 | 3.14 | 1 | -1.57 |
| 5 | 0.32 | -0.25 | 0.16 | 3 | 0.265 | -0.84 |
代入我们的程序，得到结果如下：
| 序号 | \(\theta_1\) | \(\theta_2\) | \(\theta_3\) | \(\theta_4\) | \(\theta_5\) | \(\theta_6\) |
|---|---|---|---|---|---|---|
| 1 | 1.0469159881245618 | -3.6848268488640077 | -0.5314544325246965 | 4.739457934996882 | 0.523908606235766 | 0.6985405717445783 |
| 2 | 1.5715257919759826 | -3.602109744046789 | -0.6608338908729985 | 5.309782428345745 | 0.5236351560106807 | 0.0013221366719161028 |
| 3 | 0.638109037379833 | -3.9315902306928483 | -1.3432621283836084 | 6.091162835543736 | -0.010496018243560683 | 0.010128409252622487 |
| 4 | -0.06591845703403737 | -3.9689490669809517 | -0.8245292249247629 | 5.365232265305782 | 0.054596781589503346 | -0.0361945809572239 |
| 5 | -0.7356518980607447 | -4.176588341280122 | -1.0532070322734461 | 6.510149968753879 | 0.07485626641182383 | -0.01280542967480542 |
将以上的点代入仿真程序，可以得到与末端位姿相同的结果:
仿真结果1：
[0.11699999999999991, 0.33399999999999985, 0.020422614535513077, -2.019, -0.05800000000000007, -2.19]
仿真结果2：
[-0.06600000000000003, 0.33899999999999997, -0.03517289232717361, -2.6180000000000003, -0.5239999999999999, -3.141]
仿真结果3：
[0.3, 0.25000000000000006, 0.18088897246534913, -2.640000000000001, 0.589999999999999, -2.349999999999999]
仿真结果4：
[0.42000000000000004, 1.680513367352532e-17, 0.1371152126509033, 3.14, 1.0000000000000004, -1.5]
仿真结果5：
[0.31999999999999995, -0.25, 0.1599999999999998, 3.0, 0.265, -0.8399999999999997]
说明我们的程序是正确的。