小小林熬夜学编程

这个屌丝很懒，什么也没留下！

热门标签

通用DH的逆运动学符号运算_dh矩阵求逆

作者：小小林熬夜学编程 | 2024-02-16 21:10:43

踩

dh矩阵求逆

通用HD的逆运动学符号运算

这里想说一个问题呀，初学东西还是应该首先看认可度高的书或者官方文档，至少要知道他们所用方法的思想是什么，不要动不动就上博客知乎，尤其csdn对于我是一个初学记录与分享平台，相信很多朋友和我一样，但是初学记录的东西不一定正确、全面或者方法不是最佳的，很可能误导小白。自己不爱看书做事急躁，科研习惯差，完全是这种坏习惯的受害者，还是应该在知道经典方法的前提下根据实际情况应用自己的方法

最近又有一个活：对我们自己设计的机械臂进行逆运动学推导（这次是修正DH ）

附上一个我认为的修正DH最强表述，来自B站台湾大学林沛群老师课程

在这里插入图片描述
对于修正DH

正运动学：
在这里插入图片描述
利用符号运算计算 Transformation Matrix

import sympy as sym
from sympy import sin,cos
import math
from sympy.matrices import Matrix 

pi=math.pi
#DH参数
a=[0,0,280,100,0,0]
alpha=[0,-pi/2 ,0,pi/2,-pi/2 ,pi/2]
d=[210,0,0,-220,0,180]

theta = sym.symbols('theta')


for i in range(6):
    
    cos_alpha=round(cos(alpha[i]), 4)
    sin_alpha=round(sin(alpha[i]), 4)
    theta = sym.symbols('theta'+chr(i+49))
    #除0外，di暂时用符号来表示
    if a[i]==0:
        ai=0
    else:
        a_i='a'+chr(i+49) 
        ai = sym.symbols(a_i)
    if d[i]==0:
        di=0
    else:
        d_i='d'+chr(i+49)  
        di = sym.symbols(d_i)
    T1=cos(theta)
    T2=-sin(theta)
    T3=0
    T4=ai
    T5=sin(theta)*cos_alpha
    T6=cos(theta)*cos_alpha
    T7=-sin_alpha
    T8=-sin_alpha*di
    T9=sin(theta)*sin_alpha
    T10=cos(theta)*sin_alpha
    T11=cos_alpha
    T12=cos_alpha*di
    T13=0
    T14=0
    T15=0
    T16=1

    m=Matrix([[T1,T2,T3,T4],[T5,T6,T7,T8],[T9,T10,T11,T12],[T3,T14,T15,T16]])
    print('\n') 
    print(sym.latex(m))
    print('\n') 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52

$^{0}_1T=\left[$

\begin{matrix} \cos (θ_{1}) & - \sin (θ_{1}) & 0 & 0 \\ \sin (θ_{1}) & \cos (θ_{1}) & 0 & 0 \\ 0 & 0 & 1 & d_{1} \\ 0 & 0 & 0 & 1 \end{matrix}

$\begin{matrix}\cos{\left(\theta_{1} \right)} & - \sin{\left(\theta_{1} \right)} & 0 & 0\\\sin{\left(\theta_{1} \right)} & \cos{\left(\theta_{1} \right)} & 0 & 0\\0 & 0 & 1 & d_{1}\\0 & 0 & 0 & 1\end{matrix}$ \right]

_{1}^{0} T = ⎣ ⎢ ⎢ ⎡ cos (θ_{1}) sin (θ_{1}) 00 - sin (θ_{1}) cos (θ_{1}) 00 0010 00 d_{1} 1 ⎦ ⎥ ⎥ ⎤

$^{1}_2T=\left[$

\begin{matrix} \cos (θ_{2}) & - \sin (θ_{2}) & 0 & 0 \\ 0 & 0 & 1.0 & 0 \\ - 1.0 \sin (θ_{2}) & - 1.0 \cos (θ_{2}) & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}

$\begin{matrix}\cos{\left(\theta_{2} \right)} & - \sin{\left(\theta_{2} \right)} & 0 & 0\\0 & 0 & 1.0 & 0\\- 1.0 \sin{\left(\theta_{2} \right)} & - 1.0 \cos{\left(\theta_{2} \right)} & 0 & 0\\0 & 0 & 0 & 1\end{matrix}$ \right]

_{2}^{1} T = ⎣ ⎢ ⎢ ⎡ cos (θ_{2}) 0 - 1.0 sin (θ_{2}) 0 - sin (θ_{2}) 0 - 1.0 cos (θ_{2}) 0 0 1.0 00 0001 ⎦ ⎥ ⎥ ⎤

$^{2}_3T=\left[$

\begin{matrix} \cos (θ_{3}) & - \sin (θ_{3}) & 0 & a_{3} \\ \sin (θ_{3}) & \cos (θ_{3}) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}

$\begin{matrix}\cos{\left(\theta_{3} \right)} & - \sin{\left(\theta_{3} \right)} & 0 & a_{3}\\\sin{\left(\theta_{3} \right)} & \cos{\left(\theta_{3} \right)} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}$ \right]

_{3}^{2} T = ⎣ ⎢ ⎢ ⎡ cos (θ_{3}) sin (θ_{3}) 00 - sin (θ_{3}) cos (θ_{3}) 00 0010 a_{3} 001 ⎦ ⎥ ⎥ ⎤

$^{3}_4T=\left[$

\begin{matrix} \cos (θ_{4}) & - \sin (θ_{4}) & 0 & a_{4} \\ 0 & 0 & - 1.0 & - 1.0 d_{4} \\ 1.0 \sin (θ_{4}) & 1.0 \cos (θ_{4}) & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}

$\begin{matrix}\cos{\left(\theta_{4} \right)} & - \sin{\left(\theta_{4} \right)} & 0 & a_{4}\\0 & 0 & -1.0 & - 1.0 d_{4}\\1.0 \sin{\left(\theta_{4} \right)} & 1.0 \cos{\left(\theta_{4} \right)} & 0 & 0\\0 & 0 & 0 & 1\end{matrix}$ \right]

_{4}^{3} T = ⎣ ⎢ ⎢ ⎡ cos (θ_{4}) 0 1.0 sin (θ_{4}) 0 - sin (θ_{4}) 0 1.0 cos (θ_{4}) 0 0 - 1.0 00 a_{4} - 1.0 d_{4} 01 ⎦ ⎥ ⎥ ⎤

$^{4}_5T=\left[$

\begin{matrix} \cos (θ_{5}) & - \sin (θ_{5}) & 0 & 0 \\ 0 & 0 & 1.0 & 0 \\ - 1.0 \sin (θ_{5}) & - 1.0 \cos (θ_{5}) & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}

$\begin{matrix}\cos{\left(\theta_{5} \right)} & - \sin{\left(\theta_{5} \right)} & 0 & 0\\0 & 0 & 1.0 & 0\\- 1.0 \sin{\left(\theta_{5} \right)} & - 1.0 \cos{\left(\theta_{5} \right)} & 0 & 0\\0 & 0 & 0 & 1\end{matrix}$ \right]

_{5}^{4} T = ⎣ ⎢ ⎢ ⎡ cos (θ_{5}) 0 - 1.0 sin (θ_{5}) 0 - sin (θ_{5}) 0 - 1.0 cos (θ_{5}) 0 0 1.0 00 0001 ⎦ ⎥ ⎥ ⎤

$^{5}_6T=\left[$

\begin{matrix} \cos (θ_{6}) & - \sin (θ_{6}) & 0 & 0 \\ 0 & 0 & - 1.0 & - 1.0 d_{6} \\ 1.0 \sin (θ_{6}) & 1.0 \cos (θ_{6}) & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}

$\begin{matrix}\cos{\left(\theta_{6} \right)} & - \sin{\left(\theta_{6} \right)} & 0 & 0\\0 & 0 & -1.0 & - 1.0 d_{6}\\1.0 \sin{\left(\theta_{6} \right)} & 1.0 \cos{\left(\theta_{6} \right)} & 0 & 0\\0 & 0 & 0 & 1\end{matrix}$ \right]

_{6}^{5} T = ⎣ ⎢ ⎢ ⎡ cos (θ_{6}) 0 1.0 sin (θ_{6}) 0 - sin (θ_{6}) 0 1.0 cos (θ_{6}) 0 0 - 1.0 00 0 - 1.0 d_{6} 01 ⎦ ⎥ ⎥ ⎤

$T=^{0}_1T^{1}_2T^{2}_3T^{3}_4T^{4}_5T^{5}_6T=$

[\begin{matrix} n_{x} & o_{x} & a_{x} & p_{x} \\ n_{y} & o_{y} & a_{y} & p_{y} \\ n_{z} & o_{z} & a_{z} & p_{z} \\ 0 & 0 & 0 & 1 \end{matrix}]

$\begin{bmatrix} n_x& o_x & a_x &p_x\\ n_y & o_y& a_y & p_y \\ n_z & o_z & a_z & p_z \\ 0 &0 &0 &1 \end{bmatrix}$

⎣ ⎢ ⎢ ⎡ n_{x} n_{y} n_{z} 0 o_{x} o_{y} o_{z} 0 a_{x} a_{y} a_{z} 0 p_{x} p_{y} p_{z} 1 ⎦ ⎥ ⎥ ⎤

旋转矩阵连乘后的矩阵取每个元素：

for i in range(4):
    for j in range(4):
        print('\n') 
        print(i,j) 
        print('\n') 
        print(sym.latex(sym.simplify(m[i,j])))
1
2
3
4
5
6

（这里不知道simplfy是不是最简）得到：

$n_x=- \left(\left(1.0 \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{4} \right)} - \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{4} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)}\right) \cos{\left(\theta_{5} \right)} + 1.0 \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} + \theta_{3} \right)} \cos{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{6} \right)} - 1.0 \left(\sin{\left(\theta_{1} \right)} \cos{\left(\theta_{4} \right)} + \sin{\left(\theta_{4} \right)} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)}\right) \sin{\left(\theta_{6} \right)}$

$o_x=\left(\left(1.0 \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{4} \right)} - \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{4} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)}\right) \cos{\left(\theta_{5} \right)} + 1.0 \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} + \theta_{3} \right)} \cos{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{6} \right)} - 1.0 \left(\sin{\left(\theta_{1} \right)} \cos{\left(\theta_{4} \right)} + \sin{\left(\theta_{4} \right)} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)}\right) \cos{\left(\theta_{6} \right)}$

$a_x=- 1.0 \cdot \left(1.0 \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{4} \right)} - \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{4} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)}\right) \sin{\left(\theta_{5} \right)} + 1.0 \sin{\left(\theta_{2} + \theta_{3} \right)} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)}$

$n_y=1.0 \left(\left(\sin{\left(\theta_{1} \right)} \cos{\left(\theta_{4} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} + \sin{\left(\theta_{4} \right)} \cos{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{5} \right)} - \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} + \theta_{3} \right)}\right) \cos{\left(\theta_{6} \right)} - 1.0 \left(\sin{\left(\theta_{1} \right)} \sin{\left(\theta_{4} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} - \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{4} \right)}\right) \sin{\left(\theta_{6} \right)}$

$o_y=1.0 \left(- \left(\sin{\left(\theta_{1} \right)} \cos{\left(\theta_{4} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} + \sin{\left(\theta_{4} \right)} \cos{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{5} \right)} + \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} + \theta_{3} \right)}\right) \sin{\left(\theta_{6} \right)} + 1.0 \left(- \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{4} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} + \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{4} \right)}\right) \cos{\left(\theta_{6} \right)}$

$a_y=1.0 \left(\sin{\left(\theta_{1} \right)} \cos{\left(\theta_{4} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} + \sin{\left(\theta_{4} \right)} \cos{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{5} \right)} + 1.0 \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} + \theta_{3} \right)} \cos{\left(\theta_{5} \right)}$

$n_z=- 1.0 \left(\sin{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} + \sin{\left(\theta_{2} + \theta_{3} \right)} \cos{\left(\theta_{4} \right)} \cos{\left(\theta_{5} \right)}\right) \cos{\left(\theta_{6} \right)} + 1.0 \sin{\left(\theta_{4} \right)} \sin{\left(\theta_{6} \right)} \sin{\left(\theta_{2} + \theta_{3} \right)}$

$o_z=1.0 \left(\sin{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} + \sin{\left(\theta_{2} + \theta_{3} \right)} \cos{\left(\theta_{4} \right)} \cos{\left(\theta_{5} \right)}\right) \sin{\left(\theta_{6} \right)} + 1.0 \sin{\left(\theta_{4} \right)} \sin{\left(\theta_{2} + \theta_{3} \right)} \cos{\left(\theta_{6} \right)}$

$a_z=- 1.0 \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} + \theta_{3} \right)} \cos{\left(\theta_{4} \right)} + 1.0 \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)}$

$p_x=a_{3} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} \right)} + a_{4} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} + 1.0 d_{4} \sin{\left(\theta_{2} + \theta_{3} \right)} \cos{\left(\theta_{1} \right)} - 1.0 d_{6} \left(\left(1.0 \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{4} \right)} - \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{4} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)}\right) \sin{\left(\theta_{5} \right)} - 1.0 \sin{\left(\theta_{2} + \theta_{3} \right)} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)}\right)$

$p_y=a_{3} \sin{\left(\theta_{1} \right)} \cos{\left(\theta_{2} \right)} + a_{4} \sin{\left(\theta_{1} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} + 1.0 d_{4} \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} + \theta_{3} \right)} + 1.0 d_{6} \left(\left(\sin{\left(\theta_{1} \right)} \cos{\left(\theta_{4} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} + \sin{\left(\theta_{4} \right)} \cos{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{5} \right)} + \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} + \theta_{3} \right)} \cos{\left(\theta_{5} \right)}\right)$

$p_z=- 1.0 a_{3} \sin{\left(\theta_{2} \right)} - 1.0 a_{4} \sin{\left(\theta_{2} + \theta_{3} \right)} + 1.0 d_{1} + 1.0 d_{4} \cos{\left(\theta_{2} + \theta_{3} \right)} - 1.0 d_{6} \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} + \theta_{3} \right)} \cos{\left(\theta_{4} \right)} + 1.0 d_{6} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)}$

正运动学

我们由末端位置与（ZYZ）姿态欧拉角和旋转矩阵的关系求解正运动学

[\begin{matrix} c_{α} c_{β} c_{γ} - s_{α} s_{γ} & - c_{α} c_{β} s_{γ} - s_{α} c_{γ} & c_{α} s_{β} & p_{x} \\ s_{α} c_{β} c_{γ} + c_{α} s_{γ} & - s_{α} c_{β} c_{γ} + c_{α} c_{γ} & s_{α} s_{β} & p_{y} \\ - s_{β} c_{γ} & s_{β} s_{γ} & c_{β} & p_{z} \\ 0 & 0 & 0 & 1 \end{matrix}]

$\begin{bmatrix} c_\alpha c_\beta c_\gamma-s_\alpha s_\gamma& -c_\alpha c_\beta s_\gamma-s_\alpha c_\gamma &c_\alpha s_\beta &p_x\\ s_\alpha c_\beta c_\gamma+c_\alpha s_\gamma& -s_\alpha c_\beta c_\gamma+c_\alpha c_\gamma& s_\alpha s_\beta & p_y \\ -s_\beta c_\gamma& s_\beta s_\gamma &c_\beta & p_z \\ 0 &0 &0 &1 \end{bmatrix}$

T = ⎣ ⎢ ⎢ ⎡ c_{α} c_{β} c_{γ} - s_{α} s_{γ} s_{α} c_{β} c_{γ} + c_{α} s_{γ} - s_{β} c_{γ} 0 - c_{α} c_{β} s_{γ} - s_{α} c_{γ} - s_{α} c_{β} c_{γ} + c_{α} c_{γ} s_{β} s_{γ} 0 c_{α} s_{β} s_{α} s_{β} c_{β} 0 p_{x} p_{y} p_{z} 1 ⎦ ⎥ ⎥ ⎤

[\begin{matrix} n_{x} & o_{x} & a_{x} & p_{x} \\ n_{y} & o_{y} & a_{y} & p_{y} \\ n_{z} & o_{z} & a_{z} & p_{z} \\ 0 & 0 & 0 & 1 \end{matrix}]

逆运动学

角度求解 $\theta_1$ :
由：

$^{0}_1T^{-1}T=^{1}_2T ^{2}_3T^{3}_4T^{4}_5T^{5}_6T$

[\begin{matrix} n_{x} c_{1} + n_{y} s_{1} & o_{x} c_{1} + o_{y} s_{1} & a_{x} c_{1} + a_{y} s_{1} & p_{x} c_{1} + p_{y} s_{1} \\ - n_{x} s_{1} + n_{y} c_{1} & - o_{x} s_{1} + o_{y} c_{1} & - a_{x} s_{1} + a_{y} c_{1} & - p_{x} s_{1} + p_{y} c_{1} \\ n_{z} & o_{z} & a_{z} & - d_{1} + p_{z} \\ 0 & 0 & 0 & 1 \end{matrix}]

$\begin{bmatrix} n_xc_1+n_ys_1 & o_xc_1+o_ys_1&a_xc_1+a_ys_1 &p_xc_1+p_ys_1 \\ -n_xs_1+n_yc_1 &- o_xs_1+o_yc_1&-a_xs_1+a_yc_1 &-p_xs_1+p_yc_1 \\ n_z &o_z&a_z&-d_1+p_z \\ 0 &0&0 &1 \\ \end{bmatrix}$ =

= ⎣ ⎢ ⎢ ⎡ n_{x} c_{1} + n_{y} s_{1} - n_{x} s_{1} + n_{y} c_{1} n_{z} 0 o_{x} c_{1} + o_{y} s_{1} - o_{x} s_{1} + o_{y} c_{1} o_{z} 0 a_{x} c_{1} + a_{y} s_{1} - a_{x} s_{1} + a_{y} c_{1} a_{z} 0 p_{x} c_{1} + p_{y} s_{1} - p_{x} s_{1} + p_{y} c_{1} - d_{1} + p_{z} 1 ⎦ ⎥ ⎥ ⎤ =

[\begin{matrix} s_{5} c_{6} s_{23} - c_{4} c_{5} c_{6} c_{23} - s_{4} s_{6} c_{23} & s_{5} s_{6} s_{23} - c_{4} c_{5} s_{6} c_{23} - s_{4} c_{6} c_{23} & s_{5} c_{4} c_{23} + s_{23} c_{5} & a_{3} c_{2} + a_{4} c_{23} + d_{4} s_{23} + d_{6} s_{5} c_{4} c_{23} + d_{6} s_{23} c_{5} \\ s_{4} c_{5} c_{6} + s_{6} c_{4} & s_{4} c_{5} s_{6} + c_{6} c_{4} & s_{4} s_{5} & d_{6} s_{4} s_{5} \\ s_{5} c_{6} c_{23} + c_{4} c_{5} c_{6} s_{23} + s_{4} s_{6} s_{23} & s_{5} s_{6} c_{23} + c_{4} c_{5} s_{6} s_{23} + s_{4} c_{6} s_{23} & s_{5} c_{4} s_{23} + c_{5} c_{23} & a_{3} s_{2} - a_{4} s_{23} + d_{4} c_{23} - d_{6} s_{5} c_{4} s_{23} + d_{6} c_{5} c_{23} \\ 0 & 0 & 0 & 1 \end{matrix}]

$\begin{bmatrix}s_5c_6s_{23}-c_4c_5c_6c_{23}-s_4s_6c_{23}& s_5s_6s_{23}-c_4c_5s_6c_{23}-s_4c_6c_{23}& s_5c_4c_{23}+s_{23}c_5& a_3c_2+a_4c_{23}+d_4s_{23}+d_6s_5c_4c_{23}+d_6s_{23}c_5 \\ s_4c_5c_6+s_6c_4& s_4c_5s_6+c_6c_4& s_4s_5& d_6s_4s_5\\ s_5c_6c_{23}+c_4c_5c_6s_{23}+s_4s_6s_{23} & s_5s_6c_{23}+c_4c_5s_6s_{23}+s_4c_6s_{23}& s_5c_4s_{23}+c_5c_{23}& a_3s_2-a_4s_{23}+d_4c_{23}-d_6s_5c_4s_{23}+d_6c_5c_{23}\\ 0 &0&0 &1 \\ \end{bmatrix}$

⎣ ⎢ ⎢ ⎡ s_{5} c_{6} s_{23} - c_{4} c_{5} c_{6} c_{23} - s_{4} s_{6} c_{23} s_{4} c_{5} c_{6} + s_{6} c_{4} s_{5} c_{6} c_{23} + c_{4} c_{5} c_{6} s_{23} + s_{4} s_{6} s_{23} 0 s_{5} s_{6} s_{23} - c_{4} c_{5} s_{6} c_{23} - s_{4} c_{6} c_{23} s_{4} c_{5} s_{6} + c_{6} c_{4} s_{5} s_{6} c_{23} + c_{4} c_{5} s_{6} s_{23} + s_{4} c_{6} s_{23} 0 s_{5} c_{4} c_{23} + s_{23} c_{5} s_{4} s_{5} s_{5} c_{4} s_{23} + c_{5} c_{23} 0 a_{3} c_{2} + a_{4} c_{23} + d_{4} s_{23} + d_{6} s_{5} c_{4} c_{23} + d_{6} s_{23} c_{5} d_{6} s_{4} s_{5} a_{3} s_{2} - a_{4} s_{23} + d_{4} c_{23} - d_{6} s_{5} c_{4} s_{23} + d_{6} c_{5} c_{23} 1 ⎦ ⎥ ⎥ ⎤

得：

$d_6(-a_xs_1+a_yc_1)=-p_xs_1+p_yc_1$

$a_xd_6+p_x)s_1-(p_y-d_6a_y)c_1=0$

总能找到一个分母 $m$

$sin{\phi}=\frac{-a_xd_6+p_x}{m}，cos{\phi}=\frac{p_y-d_6a_y}{m}$
$sin(\theta_1-\phi)=0$
$cos(\theta_1-\phi)=±1$
$\theta_1=atan2(sin({\phi})，cos(\phi))+atan2(0，±1)$

角度求解 $\theta_2$ :

…
同理求解 $\theta_3、\theta_4.......\theta_6$

通用的代码的应用就到这里，感兴趣的同学可以尝试继续用smypy解出 $\theta$

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小小林熬夜学编程/article/detail/96680