python机械臂仿真_双连杆机械臂运动学正向和逆向 Python 仿真！

DH(Denavit-Hartenberg)方法

DH 方法描述了相邻坐标系之间的旋转与移动的关系，只需要找到 4 个参数，以基座坐标 (x0,y0,z0)(x0,y0,z0) 为参考，就可以依次求出相连接的连杆坐标系 (xi,yi,zi)(xi,yi,zi) 和末端执行器坐标系 (xn,yn,zn)(xn,yn,zn) 的远点与方向。

DH 方法要求相邻坐标系之间满足下面约定xixi 和 zi−1zi−1 轴垂直；

xixi 和 zi−1zi−1 轴相交；

4 个 DH 参数分别为：关节转角(joint angle) θiθi

连杆扭角(link twist) αiαi

连杆距离(link offset) didi

连杆长度(link length) aiai

正运动学公式

逆运动学公式

θ2=tan−1(sinθ2cosθ2)=tan−1⎛⎜⎝−Sθ1p0x+Cθ1p0yl2Cθ1p0x+Sθ1p0y−l1l2⎞⎟⎠=tan−1(−Sθ1p0x+Cθ1p0yCθ1p0x+Sθ1p0y−l1)θ2=tan−1⁡(sin⁡θ2cos⁡θ2)=tan−1⁡(−S1θpx0+C1θpy0l2C1θpx0+S1θpy0−l1l2)=tan−1⁡(−S1θpx0+C1θpy0C1θpx0+S1θpy0−l1)

平面连杆正运动学仿真

import numpy as np from numpy import sin, cos import matplotlib.pyplot as plt def dhmat(delta, d, a, alpha): return np.array([[cos(delta), -sin(delta)*cos(alpha