博主最近也是准备研究生开学了，由于老师还未叫我去实验室，于是自己在家里自学了一些机械臂相关的知识，其实这个博客网站早在我大三的时候就依旧搭建好了，但是当时也是被期末考试虞姬后面的保研的东西忙前忙后，最后导致这个网站被搁置了两年。那为什么我又会再次打开这个网站，在屏幕前敲起键盘，想把自己学习到的一些知识做成博客呢？最近也不知道是怎么了，一个从小都不喜欢看书的人，开始看起书来了，给大家分享一本书，叫做《认知觉醒》，我也是看了这本书之后才重新完善了我的博客网站，并且开始写博客，里面讲到一个很重要的学习方法就是需要输出，学习是需要反馈的，那么反馈从何而来呢，得先有输出才能有反馈，作为一名工科学生，手把手去教师弟师妹的时刻是比较少的，于是写博客就成为一个很好的输出形式，既可以将自己一段时间来学习的东西以自己的语言给讲述出来，还能与志同道合的网友一起学习交流，何乐而不为呢？其实之前在开始做博客的时候就有同学觉得我做这个也没啥用，写了之后也不知道有没有人看，自己花费心思完成的再好，最后也是自己以后可能要用到的时候看一下，当时的我又被这个想法给动摇了，想着也是。但是最近觉得并不是如此，写博客想要一下子很多人看，于我而言是不可能的，但是在这个过程中，我可能会对我所学习到的知识有了更加深刻的认识，觉得花这个时间还是值得的，最后也能构成自己的知识库。本人写作水平有限，还望广大网友谅解，希望大家也能找到属于自己的输出方式，与大家共勉，共同加油！

在学习完机械臂的运动学、动力学等内容后，直接对照着视频中的公式进行推导感觉太枯燥了，于是在AI的帮助下，使用python进行两自由度的机械臂动力学仿真实验，之所以选择做两自由度呢，是因为这个难度他的运动学公式可以使用手推，比较好的验证。OK，下面我就讲解一下我的完整过程吧！

安装需要的库

本次仿真需要用到2个python库，包括：numpy、matplotlib，如果在运行代码过程中报错，那么只需要运行下面的代码即可安装

1
pip install numpy matplotlib

编写主体代码

创建 2-DOF 机械臂类

这个2-DOF机械臂类主要包含机械臂的建模参数、正逆向运动学求解、动力学建模这几个部分，下面就对该类内的方法进行一一讲解以及源码展示。

初始化

我们需要进行动力学的仿真，因此需要传入质量，如果只是单纯的运动学仿真可以不传入质量部分。

1
2
3
def __init__(self, l1=1.0, l2=1.0, m1=1.0, m2=1.0):
        self.l1 ,self.l2 = l1, l2	# l1, l2 分别代表大臂和小臂的长度
        self.m1 ,self.m2 = m1 ,m2	# m 代表质量

正向运动学

正向运动学主要负责将传入的关节空间的角度转换为末端执行器在笛卡尔空间下的坐标。这部分的解算比较好推导，大家可以拿出纸笔画一下机械臂的结构就能理解下面的代码。

base、elbow、end分别表示基座、肘部、末端执行器，正向运动学就是需要解算出肘部和末端执行器的位置，因此由下图我们可以得到elbow的位置为： $$ \begin{aligned} x_{elbow} &= l_1 cos(q_1) \\ y_{elbow} &= l_1 sin(q_1) \end{aligned} $$ 同理我们可以得出末端执行器的位置： $$ \begin{aligned} x_{end} &= x_{elbow} + l_2 cos(q_2) \\ y_{end} &= y_{elbow} + l_2 sin(q_2) \end{aligned} $$

以上就是两自由度机械臂的正向运动学推导了，更多自由度的机械臂推导方法也一致的。

下面直接展示代码部分：

1
2
3
4
5
6
7
def forward_kinematics(self, theta):
        x1 = self.l1 * np.cos(theta[0])
        y1 = self.l1 * np.sin(theta[0])
        x2 = x1 + self.l2 * np.cos(theta[0] + theta[1])
        y2 = y1 + self.l2 * np.sin(theta[0] + theta[1])

        return np.array([0, x1, x2]), np.array([0, y1, y2])

逆向运动学

逆向运动学就是输入为笛卡尔空间的坐标，需要求解出关节空间的角度，这个求解过程是要比正向难很多的，特别是当自由度更加多的时候。我们可以先用两自由度的求解理解一下。

我们依然可以使用正向运动学的示意图进行解算，首先计算出末端执行器到原点的距离： r2 = x2 + y2 计算出距离之后，先要进行可达空间的判断，

1
2
if (r > self.l1 + self.l2) or (r < self.l1 - self.l2):
            print(f"超出工作空间")

当末端位置在可达空间内时才进行下一步的解算，得到末端执行器到原点的距离后，由机械臂大臂和小臂组合成的三角形，三边长度都已经知道，则可以使用余弦公式计算出 q2 的大小 cos(q2) = r2 − l12 − l22/(2l1l2) 由于机械臂存在两个解，也就是肘部的朝向问题，默认肘部朝上时，那么直接反余弦求解角度即可，如果设置肘部朝下则再计算出角度后变号即可。

接下来进行q1的解算，解算这个关节角度需要求出两个辅助角，α 和 β ,我们可以借助q2求解 α 、β $$ \begin{aligned} tan(\alpha) &= \frac{l_2 sin(q_2)} {l_1 + l_2 sin(q_2)} \\ tan(\beta) &= \frac {y} {x} \\ q_2 &= \beta - \alpha \end{aligned} $$ 求解出两个辅助角度之后，将他们相减即可完成q1的解算。

下面直接展示代码部分：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
def invert_kinematics(self, xe, ye, elbow_up=True):
        r = xe*xe + ye*ye
        if (r > self.l1 + self.l2) or (r < self.l1 - self.l2):
            print(f"超出工作空间")
            return -1
        c2 = r - np.power(self.l1, 2) - np.power(self.l2, 2) / (2 * self.l1 * self.l2)
        if elbow_up == True:
            s2 = np.sqrt(1 - np.power(c2,2))
        else:
            s2 = - np.sqrt(1 - np.power(c2,2))
        theta2 = np.arctan2(s2, c2)
        theta1 = np.arctan2(ye, xe) - np.arctan2(self.l2 * np.sin(theta2), self.l1 + self.l2 * np.cos(theta2))

        return np.array([theta1, theta2])

动力学建模

动力学建模主要工作是得出惯性矩阵、科氏力/离心力矩阵和重力矩阵三个矩阵。

动力学的建模有两种方法：

  1. 朗格朗日法（这个方法使用能量守恒进行分析），这个方法适合理论分析和低自由度。

  2. 牛顿-欧拉法（这个方法使用牛顿方程和欧拉方程进行分析），这个方法适合多自由度机械臂以及工程实现。

由于这个求解的过程是比较复杂的，如果需要的话，可以在评论区评论，我找时间出一下动力学建模的过程文章，下面直接展示代码了。（此处将机械臂认为是一个点质量模型，所有的质量都聚焦在关节处，来简化计算）

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
def Dynamics_matrices(self, theta, dtheta):
    theta1, theta2 = theta
    dtheta1, dtheta2 = dtheta

    l1, l2, m1, m2= self.l1, self.l2, self.m1, self.m2
    '''
    计算M、C、G矩阵
    '''
    M11 = m1*l1**2+m2*(l1+l2+2*l1*l2*np.cos(theta2))
    M12 = M21 = m2*l2**2+m2*l1*l2*np.cos(theta2)
    M22 = m2*l2**2

    C11 = -m2*l1*l2*np.sin(theta2)*dtheta2
    C12 = -m2*l1*l2*np.sin(theta2)*(dtheta1+dtheta2)
    C21 = m2*l1*l2*np.sin(theta2)*dtheta1
    C22 = 0

    G11 = (m1+m2)*9.81*l1*np.cos(theta1)+m2*9.81*l2*np.cos(theta1+theta2)
    G21 = m2*9.81*l2*np.cos(theta2+theta1)

    M = np.array([[M11, M12], [M21, M22]])
    C = np.array([[C11, C12], [C21, C22]])
    G = np.array([G11, G21])

    return M, C, G

轨迹求解器

以上完成了机械臂的运动学、动力学求解器的编写，接下来就是轨迹规划的求解了，这里使用的是三次多项式进行轨迹求解，为什么是三次多项式呢，因为第一我们需要各个点之间的位置是连续的，第二各个接触点之间的速度是连续，不然就会出现每个点之间来个速度突变，这是不现实的，而我们的未知数有四个分别是起点位置和速度、终点位置和速度，三次多项式就有四个系数来约束方程。轨迹规划又分为关节空间、笛卡尔空间的规划，本文使用的是关节空间的规划。

轨迹规划的求解过程也是比较多，如果需要我可以单独出一片文章进行讲解，下面直接展示代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
class TrajectoryPlanner:
    @staticmethod
    def cubic_polynomial(theta0, thetaf, T, dt):
        '''三次项多项式轨迹规划'''
        # 先给出每个采样点的横坐标
        t = np.arange(0, T+dt, dt)
        # 再计算三次项的参数
        a0, a1 = theta0, 0      # 初始位置为theta，初始角速度也为0
        a2 = 3*(thetaf-theta0)/(T**2)
        a3 = -2*(thetaf-theta0)/(T**3)
        # 得出三次项轨迹计算式
        theta = a0 + a1*t + a2*t**2 + a3*t**3
        dtheta = a1 + 2*a2*t + 3*a3*t**2
        ddtheta = 2*a2 + 6*a3*t

        return theta, dtheta, ddtheta, t

力矩控制器

我们进行动力学建模后，最终需要得到的是每个电机需要的力矩，使用这个数据去控制电机的转动。得出力矩只需要将轨迹规划求解得到的角度的角速度和角加速度带入计算公式即可得到。 $$ M(q)\ddot{q} + C(q,\dot{q})\dot{q}+G(q)=\tau $$ 本文使用计算力矩的PD控制，也就是对位置误差和速度误差做了一个反馈，下面给出代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
class ComputedTorqueController:
    def __init__(self, arm, Kp=100, Kv=20):
        self.arm = arm
        self.Kp = np.diag([Kp, Kp])
        self.Kv = np.diag([Kv, Kv])

    def control(self, theta, dtheta, theta_d, dtheta_d, ddtheta_d):
        '''计算力矩'''
        e = theta_d - theta
        de = dtheta_d - dtheta

        M, C, G = arm.Dynamics_matrices(theta, dtheta)
        tau = M @ (ddtheta_d + self.Kp @ e + self.Kv @ de) + C @ dtheta_d + G
        return tau

开始仿真

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
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
#--------------------------------------仿真开始--------------------------------------------
arm = TwoLinkArm()      # 创建机械臂类
dt = 0.01               # 设置微分时间差
T = 5.0                 # 总的运动时间

# 在关节角度空间进行规划
set_theta1 = float(input('请输入theta1的目标角度:'))
set_theta2 = float(input('请输入theta2的目标角度:'))

# 初始角度和最终角度
theta1 = np.array([0, np.deg2rad(set_theta1)])
theta2 = np.array([0, np.deg2rad(set_theta2)])

# 轨迹规划
theta1_d, dtheta1_d, ddtheta1_d, t = TrajectoryPlanner.cubic_polynomial(theta1[0], theta1[1], T, dt)
theta2_d, dtheta2_d, ddtheta2_d, _ = TrajectoryPlanner.cubic_polynomial(theta2[0], theta2[1], T, dt)

# 仿真初始化参数
theta = np.array([0.1, 0.1])
dtheta = np.array([0.0, 0.0])
controller = ComputedTorqueController(arm)

# 记录数据
theta_history = [theta.copy()]
dtheta_history = [dtheta.copy()]

# 将轨迹规划完成的数据进行积分得到关节实际角度
for i in range(len(t)):
    theta_d = np.array([theta1_d[i], theta2_d[i]])
    dtheta_d = np.array([dtheta1_d[i], dtheta2_d[i]])
    ddtheta_d = np.array([ddtheta1_d[i], ddtheta2_d[i]])

    # 计算控制力距
    tau = controller.control(theta, dtheta, theta_d, dtheta_d, ddtheta_d)
    
    # 前向运动学 这部分由于我们是在电脑上仿真所以需要自己换算，实际中是机械臂反馈给我们的
    M, C, G = arm.Dynamics_matrices(theta, dtheta)
    ddtheta = np.linalg.solve(M, tau - C @ dtheta - G)

    # 积分将角度还原
    dtheta += ddtheta * dt
    theta += dtheta * dt

    theta_history.append(theta.copy())
    dtheta_history.append(dtheta.copy())

theta_history = np.array(theta_history)

# =================绘图==========================
# plt.figure(figsize=(10, 5))
# plt.xlim(-2.5, 2.5)
# plt.ylim(-2.5, 2.5)
# plt.grid(True)
# line, = plt.plot([], [], 'o-', lw=3, markersize=10)
fig, ax = plt.subplots(2,2,figsize=(12, 10))
ax1 = ax[0, 0]
ax1.set_xlim(-2.5, 2.5)
ax1.set_ylim(-2.5, 2.5)
ax1.set_aspect('equal')
ax1.grid(True)
line, = ax1.plot([], [], 'o-', lw=3, markersize=10)

def animate(i):
    x, y = arm.forward_kinematics(theta_history[i])
    line.set_data(x, y)
    return line,

ani = FuncAnimation(fig, animate, frames=len(theta_history), interval=dt*1000, blit=True)
ax1.set_title('两连杆机械臂轨迹跟踪')

# 关节角度跟踪
ax2 = ax[0, 1]
ax2.plot(t, theta1_d, 'r--', label=r'$\theta_{1d}$')
ax2.plot(t, theta2_d, 'b--', label=r'$\theta_{2d}$')
ax2.plot(t, theta_history[:-1,0], 'r-', label=r'$\theta_1$')
ax2.plot(t, theta_history[:-1,1], 'b-', label=r'$\theta_2$')
ax2.set_xlabel('时间(s)')
ax2.set_ylabel('关节角度(rad)')
ax2.legend()
ax2.grid(True)
ax2.set_title('关节角度空间轨迹跟踪')

# 跟踪误差
ax3 = ax[1,0]
e1 = theta1_d - theta_history[:-1,0]
e2 = theta2_d - theta_history[:-1,1]
ax3.plot(t, e1, 'r-', label=r'$e_1$')
ax3.plot(t, e2, 'b-', label=r'$e_2$')
ax3.set_xlabel('时间(t)')
ax3.set_ylabel('角度误差(rad)')
ax3.legend()
ax3.grid(True)
ax3.set_title('跟踪误差')

# 末端轨迹
ax4 = ax[1,1]
x_end = []
y_end = []
for th in theta_history:
    x_arr, y_arr = arm.forward_kinematics(th)
    x_end.append(x_arr[2])
    y_end.append(y_arr[2])

x_d_end = []
y_d_end = []
for i in range(len(t)):
    th = np.array([theta1_d[i], theta2_d[i]])
    x, y = arm.forward_kinematics(th)
    x_d_end.append(x[2])
    y_d_end.append(y[2])

ax4.plot(x_d_end, y_d_end, 'r-', label='期望轨迹')
ax4.plot(x_end, y_end, 'b-', label='实际轨迹')
ax4.set_xlabel('x(m)')
ax4.set_ylabel('y(m)')
ax4.legend()
ax4.set_aspect('equal')
ax4.grid(True)
ax4.set_title('末端控制器工作空间轨迹')

plt.show()

总结

这份代码其实已经过了许久了，之前没想过要写博客来着，经过这次博客编写我对以上的内容有更加熟悉了一边这个过程中也是我深入了解了一些内容，学习过程有借助AI的帮助，如果以上的内容有哪些错误的地方，可以在评论区告诉我，我没有在这个网站上放上评论区，因为搞起来有点麻烦，还没有去搞，本网站的所有文章会同步会同步更新到CSDN，如果需要评论可以去CSDN或者在github私信我！