机械臂运动学笔记(1)-正向运动学

机械臂
发布日期:   2025-04-20

Manipulator Forword kinematics Study

前言

近来笔者在完成本科毕设，用到一些与机械臂相关的知识，最近整理一下，主要分为正向运动学推力和逆向运动学分配两部分。本文先记录一下正向运动学的学习，记录了DH参数法和旋量法两种方法。本文所用的机械臂结构如下：
机械臂结构

DH参数法

D-H参数法按照设定规则为每个连杆固连了一个坐标系，之后就可以方便的描述一个连杆坐标系到相邻的下一个连杆坐标系的转换关系。实质就是把相邻坐标系的转换分解为了若干个步骤，每个步骤均只有一个参量。这几个步骤对应变换的组合就完成了相邻坐标系的变换。
DH
记录本机械臂DH参数表如下:

Joint d(mm) θ(°) a(mm) α(°)
1 77 q1 0 -90
2 0 q2-90 128 0
3 0 90 24 0
4 0 q3 124 0
5 0 q4 126 90

a是两个转轴之间的距离（Z轴），异面直线公垂线的长度，也就是连杆的长度，对于旋转关节是固定值
α是两个转轴之间的夹角
d是两个公垂线在同一个转轴上的交点之间的距离，实际上两个坐标轴，其中一个X轴转动θ角后与另一个X轴平行，两个X轴之间的距离
θ是两个公垂线之间的夹角（两个坐标系X轴的夹角），也就是转轴（关节）转动的角度。

步骤1：坐标系初始位于坐标系{A_{i-1}}，绕X_{i-1}​轴旋转α_{i-1}​，使Z轴与Z_{i}​轴平行；
步骤2：坐标系沿X_{i-1}​轴平移a_{i-1}，使Z轴与Z_{i}​轴共线；
步骤3：坐标系绕Z_{i-1}同时也是Z_{i}旋转θ_{i}​，使X轴与X_{i}​轴平行；
步骤4：坐标系沿Z_{i-1}​(同时也是Z_{i}​)平移d_{i}​，此时已变换到 A_{i}。

旋量法(POE)

与DH参数法不同，旋量法不过分考虑中间关节的变化，而是通过记录初始状态下的末端位置(用G0表示)，以及运动过程中关节角变化产生的旋量来进行正向运动学计算得到当前关节角下的末端位置的。
记录本机械臂的旋量参数表如下：

Joint ωi τi(mm) mass(kg)
1 [0 0 1] [0 0 77] 0.210
2 [0 1 0] [0 0 77] 0.146
3 [0 1 0] [24 0 205] 0.138
4 [0 1 0] [148 0 205] 0.236

首先在参考坐标系下，按照机械臂的关节运动正方向确定Z轴，之后通过用户方便自定义其余的X轴和Y轴。使用罗德里格斯公式表示绕物体坐标系的旋转轴一定角度后的旋量：

最终的末端变换矩阵为：

代码

给出旋量法正向运动学和蒙特卡洛方法的采样可达域量化代码

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
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
import numpy as np
from scipy.linalg import expm
import matplotlib.pyplot as plt
from scipy.spatial import ConvexHull
from mpl_toolkits.mplot3d.art3d import Poly3DCollection

# ===== 基础函数 =====
def hat(w):
    return np.array([
        [0, -w[2], w[1]],
        [w[2], 0, -w[0]],
        [-w[1], w[0], 0]
    ])

def twist_hat(w, v):
    return np.block([
        [hat(w), v.reshape(3, 1)],
        [np.zeros((1, 3)), 0]
    ])

def adjoint(g):
    R = g[:3, :3]
    p = g[:3, 3]
    p_hat = hat(p)
    return np.block([
        [R, np.zeros((3,3))],
        [p_hat @ R, R]
    ])

# ===== 参数设置 =====
G_0 = np.array([[1, 0, 0, 0.28],
                [0, 1, 0, 0],
                [0, 0, 1, 0.203],
                [0, 0, 0, 1]])
G_1 = np.array([[1, 0, 0, 0],
                [0, 1, 0, 0],
                [0, 0, 1, 0],
                [0, 0, 0, 1]])
G_2 = np.array([[1, 0, 0, 0],
                [0, 1, 0, 0],
                [0, 0, 1, 0.077],
                [0, 0, 0, 1]])
G_3 = np.array([[1, 0, 0, 0.024],
                [0, 1, 0, 0],
                [0, 0, 1, 0.205],
                [0, 0, 0, 1]])
G_4 = np.array([[1, 0, 0, 0.128],
                [0, 1, 0, 0],
                [0, 0, 1, 0.205],
                [0, 0, 0, 1]])
T_base_to_body = np.array([
    [1, 0, 0, 0.0055],
    [0, 1, 0, 0],
    [0, 0, 1, 0.064],
    [0, 0, 0, 1]])
R_inertia_to_robot_base=np.array([[1,0,0],
                                 [0,-1,0],
                                 [0,0,-1]])
G_list=[G_1,G_2,G_3,G_4]
w1 = np.array([0, 0, 1])
w234 = np.array([0, 1, 0])
w_list = [w1, w234, w234, w234]

r1 = np.array([0, 0, 0.077])
r2 = np.array([0, 0, 0.077])
r3 = np.array([0.024, 0, 0.205])
r4 = np.array([0.148, 0, 0.205])
r_list = [r1, r2, r3, r4]

# 构造螺旋轴 ξ
def exp_rotation(w, n):
    hat_w = hat(w)
    return np.eye(3) + np.sin(n) * hat_w + (1 - np.cos(n)) * np.dot(hat_w, hat_w)

# 平移部分
def exp_translation(w, r, n):
    hat_w = hat(w)
    term1 = np.dot(np.eye(3) - exp_rotation(w, n), np.cross(w, np.cross(r, w)))
    term2 = n * np.outer(w, w) @ np.cross(r, w)
    return term1 + term2

# 齐次变换矩阵计算
def forward_kinematics(q, w_list, r_list, G_0):
    # 初始化变换矩阵为单位矩阵
    exp_sigma = [np.eye(4) for _ in range(4)]

    for i in range(4):
        w = w_list[i]
        r = r_list[i]
        n = q[i]  # 关节角度
        exp_sigma[i][:3, :3] = exp_rotation(w, n)  # 旋转部分
        exp_sigma[i][:3, 3] = exp_translation(w, r, n)  # 平移部分
        exp_sigma[i][3, 3] = 1  # 末尾为 1，保持齐次坐标

    # 前向运动学最终结果
    T_ee_to_base = exp_sigma[0] @ exp_sigma[1] @ exp_sigma[2] @ exp_sigma[3] @ G_0
    return T_ee_to_base

# ===== 空间雅可比 =====
def compute_jacobian(q):
    n1, n2, n3, n4=q
    a = 0.12
    b = 0.124
    c = 0.128
    d = 0.024
    # ----- J_v（线速度部分）-----
    J_v = np.zeros((3, 4))

    J_v[0, 0] = -np.sin(n1) * (a * np.cos(n2 + n3 + n4) + b * np.cos(n2 + n3) + d * np.cos(n2) + c * np.sin(n2))
    J_v[0, 1] = np.cos(n1) * (-a * np.sin(n2 + n3 + n4) - b * np.sin(n2 + n3) + c * np.cos(n2) - d * np.sin(n2))
    J_v[0, 2] = np.cos(n1) * (-a * np.sin(n2 + n3 + n4) - b * np.sin(n2 + n3))
    J_v[0, 3] = -np.cos(n1) * a * np.sin(n2 + n3 + n4)

    J_v[1, 0] = np.cos(n1) * (a * np.cos(n2 + n3 + n4) + b * np.cos(n2 + n3) + d * np.cos(n2) + c * np.sin(n2))
    J_v[1, 1] = np.sin(n1) * (-a * np.sin(n2 + n3 + n4) - b * np.sin(n2 + n3) + c * np.cos(n2) - d * np.sin(n2))
    J_v[1, 2] = -np.sin(n1) * (a * np.sin(n2 + n3 + n4) + b * np.sin(n2 + n3))
    J_v[1, 3] = -np.sin(n1) * a * np.sin(n2 + n3 + n4)

    J_v[2, 0] = 0
    J_v[2, 1] = -(a * np.cos(n2 + n3 + n4) + b * np.cos(n2 + n3) + d * np.cos(n2) + c * np.sin(n2))
    J_v[2, 2] = -(a * np.cos(n2 + n3 + n4) + b * np.cos(n2 + n3))
    J_v[2, 3] = -a * np.cos(n2 + n3 + n4)

    # ----- J_w（角速度部分）-----
    J_w = np.zeros((3, 4))

    J_w[0, :] = [0, -np.sin(n1), -np.sin(n1), -np.sin(n1)]
    J_w[1, :] = [0, np.cos(n1), np.cos(n1), np.cos(n1)]
    J_w[2, :] = [1, 0, 0, 0]

    # 合并 J_v 和 J_w
    Jacobian = np.vstack((J_v, J_w))

    return J_v


# ===== 可达空间采样 =====
N = 50000
points = []
colors = []
sampled_J = []

for _ in range(N):
    q = np.array([
        np.random.uniform(0, np.pi),
        # np.random.uniform(-np.pi / 2, np.pi / 2),
        # np.random.uniform(-np.pi / 2, np.pi / 2),
        # np.random.uniform(-np.pi / 2, np.pi / 2)
        np.random.uniform(0, np.pi / 2),
        np.random.uniform(0, np.pi / 2),
        np.random.uniform(-np.pi / 2, np.pi / 2)
    ])
    g = forward_kinematics(q,w_list,r_list,G_0)
    pos = g[:3, 3]
    pos_b = T_base_to_body @ g
    pos_w = R_inertia_to_robot_base @ pos_b[:3,3]
    # if pos[2]<0:
    #     print(q)
    J = compute_jacobian(q)
    manipulability = np.sqrt(np.linalg.det(J @ J.T) + 1e-6)

    points.append(pos_w)
    colors.append(manipulability)

    if np.random.rand() < 0.002:
        sampled_J.append((pos_w, J))

points = np.array(points)
colors = np.array(colors)

# ===== 绘制速度椭球函数 =====
# def draw_ellipsoid(ax, center, cov, scale=0.05):
#     U, s, _ = np.linalg.svd(cov)
#     rx, ry, rz = scale * np.sqrt(s)
#     u = np.linspace(0, 2 * np.pi, 10)
#     v = np.linspace(0, np.pi, 10)
#     x = rx * np.outer(np.cos(u), np.sin(v))
#     y = ry * np.outer(np.sin(u), np.sin(v))
#     z = rz * np.outer(np.ones_like(u), np.cos(v))
#     for i in range(x.shape[0]):
#         for j in range(x.shape[1]):
#             [x[i,j], y[i,j], z[i,j]] = U @ np.array([x[i,j], y[i,j], z[i,j]]) + center
#     ax.plot_surface(x, y, z, color='orange', alpha=0.3)

# ===== 可视化 =====
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
p = ax.scatter(points[:, 0], points[:, 1], points[:, 2], c=colors, cmap='viridis', s=1)

# 基座位置
ax.scatter(0, 0, 0, color='red', s=100, marker='*', label='Base Position')

# # 绘制部分椭球体
# for pos, J in sampled_J:
#     draw_ellipsoid(ax, pos, J @ J.T)

# 凸包体积与边界
# hull = ConvexHull(points)
# volume = hull.volume
# for s in hull.simplices:
#     s = np.append(s, s[0])
#     ax.plot(points[s, 0], points[s, 1], points[s, 2], "k-", linewidth=0.3)

# 图设置
fig.colorbar(p, ax=ax, label="Manipulability")
ax.set_title("Reachable Workspace with Velocity Ellipsoids and Convex Boundary")
ax.set_xlabel("X [m]")
ax.set_ylabel("Y [m]")
ax.set_zlabel("Z [m]")
ax.legend()
plt.tight_layout()
plt.show()

# 输出体积
print(f"可达空间近似体积: {volume:.4f} m³")
def draw_robot_arm(ax, q, w_list, r_list, G_0, color='r'):
    T = np.eye(4)
    points = [T[:3, 3]]  # 从基座开始
    for i in range(4):
        w = w_list[i]
        r = r_list[i]
        n = q[i]
        exp_T = np.eye(4)
        exp_T[:3, :3] = exp_rotation(w, n)
        exp_T[:3, 3] = exp_translation(w, r, n)
        T = T @ exp_T
        pos=T@G_list[i]# 每个关节末端位置
        # pos=pos[:3,3]
        pos_b = T_base_to_body @ pos
        pos_w = R_inertia_to_robot_base @ pos_b[:3,3]
        points.append(pos_w)
    T = T @ G_0
    pos_b = T_base_to_body @ T
    pos_w = R_inertia_to_robot_base @ pos_b[:3,3]
    points.append(pos_w)  # 加上末端执行器
    points = np.array(points)
    ax.plot(points[:, 0], points[:, 1], points[:, 2], '-o', color=color, linewidth=2, markersize=4)

fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
q_example = np.array([0.24678797, 0.48229641, 0.66201096, 0.50968185])
# q_example = np.array([0, 0, 0, 0])
draw_robot_arm(ax, q_example, w_list, r_list, G_0, color='red')
plt.show()

总结

D-H 表示方法需要的参数比 PoE 表示方法少，但D-H 表示方法需要添加中间参考坐标系，而且该坐标系并不能随意选取，且不同人选取的方法可以不一样，这就很可能导致同样的结构不同的人描述出来的结果不统一。并且当两个旋转轴近乎平行时，用 D-H 表示方法可能会产生病态矩阵

文章作者: 郭昱鑫
文章链接: http://ggggggggx.work.gd/2025/04/20/manipulator/
版权声明: 本博客所有文章除特別声明外，均采用 CC BY 4.0 许可协议。转载请注明来源 郭昱鑫 !
机械臂
赏
  目录