机器人学导论核心公式实战：从数学推导到Python代码实现

1. 位姿描述与齐次变换矩阵

在机器人学中，描述物体在三维空间中的位置和姿态是基础中的基础。我们通常使用齐次变换矩阵来表示坐标系之间的相对关系。这个4×4的矩阵不仅包含了旋转信息，还包含了平移信息，能够将点从一个坐标系转换到另一个坐标系。

齐次变换矩阵的标准形式如下：

$$ ^A_BT = \begin{bmatrix} ^A_BR & ^AP_{Borg} \ 0 & 1 \end{bmatrix} $$

其中$^A_BR$是3×3的旋转矩阵，$^AP_{Borg}$是3×1的平移向量。让我们用Python代码来实现这个矩阵的构建和应用：

import numpy as np def rotation_matrix(roll, pitch, yaw): """根据欧拉角计算旋转矩阵""" R_x = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]) R_y = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]]) R_z = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]]) return R_z @ R_y @ R_x def homogeneous_transform(rotation, translation): """构建齐次变换矩阵""" T = np.eye(4) T[:3, :3] = rotation T[:3, 3] = translation return T # 示例：坐标系B相对于坐标系A的旋转(30°,45°,60°)和平移(1,2,3) R = rotation_matrix(np.radians(30), np.radians(45), np.radians(60)) T = homogeneous_transform(R, [1, 2, 3]) print("齐次变换矩阵:\n", T)

提示：在实际应用中，齐次变换矩阵的乘法对应着坐标系的连续变换。例如，$^A_CT = ^A_BT \cdot ^B_CT$表示从坐标系C到A的变换可以通过先到B再到A来实现。

2. 连杆变换与正运动学

机械臂的正运动学问题可以描述为：给定各关节的角度，计算机械臂末端执行器的位姿。解决这个问题的关键在于理解Denavit-Hartenberg(D-H)参数和连杆变换。

D-H参数法用四个参数描述相邻连杆坐标系之间的关系：

• $a_i$：连杆长度
• $\alpha_i$：连杆扭角
• $d_i$：连杆偏距
• $\theta_i$：关节角度

基于D-H参数的连杆变换矩阵为：

$$ ^{i-1}_iT = \begin{bmatrix} c\theta_i & -s\theta_i c\alpha_i & s\theta_i s\alpha_i & a_i c\theta_i \ s\theta_i & c\theta_i c\alpha_i & -c\theta_i s\alpha_i & a_i s\theta_i \ 0 & s\alpha_i & c\alpha_i & d_i \ 0 & 0 & 0 & 1 \end{bmatrix} $$

让我们实现一个2自由度机械臂的正运动学计算：

def dh_transform(a, alpha, d, theta): """根据D-H参数计算单个连杆的变换矩阵""" ct = np.cos(theta) st = np.sin(theta) ca = np.cos(alpha) sa = np.sin(alpha) return np.array([ [ct, -st*ca, st*sa, a*ct], [st, ct*ca, -ct*sa, a*st], [0, sa, ca, d], [0, 0, 0, 1] ]) # 2自由度机械臂的D-H参数 dh_params = [ {'a': 1, 'alpha': 0, 'd': 0, 'theta': np.radians(30)}, # 关节1 {'a': 1, 'alpha': 0, 'd': 0, 'theta': np.radians(45)}, # 关节2 ] # 计算正运动学 T_total = np.eye(4) for params in dh_params: T_i = dh_transform(**params) T_total = T_total @ T_i print("末端执行器位姿:\n", T_total) print("末端位置:", T_total[:3, 3])

3. 逆运动学解析解

逆运动学是正运动学的逆问题：给定末端执行器的期望位姿，求解各关节的角度。对于6自由度机械臂，逆运动学通常有多个解，我们需要选择最合适的解。

以常见的PUMA型机械臂为例，我们可以推导出解析解。下面是求解第一个关节角度$\theta_1$的示例代码：

def inverse_kinematics_puma(T_06): """PUMA型机械臂逆运动学解析解""" # 提取目标位姿的位置和方向 P_05 = T_06[:3, 3] - T_06[:3, 2] * d6 # d6是工具长度 # 求解θ1 theta1 = np.arctan2(P_05[1], P_05[0]) theta1_alt = np.arctan2(-P_05[1], -P_05[0]) # 这里只展示θ1的计算，实际还需要计算θ2,θ3等 return theta1, theta1_alt # 示例目标位姿 T_desired = np.array([ [0, -1, 0, 1], [1, 0, 0, 1], [0, 0, 1, 0.5], [0, 0, 0, 1] ]) theta1, theta1_alt = inverse_kinematics_puma(T_desired) print("θ1解1:", np.degrees(theta1), "度") print("θ1解2:", np.degrees(theta1_alt), "度")

注意：逆运动学解析解通常只适用于特定结构的机械臂。对于更复杂的机械臂，可能需要使用数值方法求解。

4. 速度运动学与雅可比矩阵

雅可比矩阵建立了关节空间速度与笛卡尔空间速度之间的关系：

$$ v = J(q)\dot{q} $$

其中$v$是末端执行器的速度向量，$\dot{q}$是关节速度向量，$J(q)$是雅可比矩阵。

对于2自由度平面机械臂，我们可以推导其雅可比矩阵：

def jacobian_2dof(theta1, theta2, l1=1, l2=1): """计算2自由度平面机械臂的雅可比矩阵""" J = np.zeros((2, 2)) J[0, 0] = -l1*np.sin(theta1) - l2*np.sin(theta1+theta2) J[0, 1] = -l2*np.sin(theta1+theta2) J[1, 0] = l1*np.cos(theta1) + l2*np.cos(theta1+theta2) J[1, 1] = l2*np.cos(theta1+theta2) return J # 示例：在关节角度为30°和45°时的雅可比矩阵 theta1 = np.radians(30) theta2 = np.radians(45) J = jacobian_2dof(theta1, theta2) print("雅可比矩阵:\n", J) # 计算给定关节速度时的末端速度 q_dot = np.array([0.5, 1.0]) # 关节1和2的速度(rad/s) v = J @ q_dot print("末端速度:", v)

雅可比矩阵在机器人控制中非常重要，它不仅可以用于速度映射，还可以用于力映射和奇异点分析。

5. 动力学建模与运动控制

机器人动力学描述了机械臂的运动与作用力/力矩之间的关系。常用的动力学方程有：

$$ \tau = M(q)\ddot{q} + C(q,\dot{q}) + G(q) $$

其中：

• $\tau$是关节力矩向量
• $M(q)$是质量矩阵
• $C(q,\dot{q})$是科里奥利力和向心力项
• $G(q)$是重力项

虽然完整的动力学模型比较复杂，但我们可以实现一个简单的PD控制器来稳定机械臂：

def pd_control(q_current, q_desired, q_dot_current, q_dot_desired, Kp, Kd): """简单的PD控制器""" error = q_desired - q_current error_dot = q_dot_desired - q_dot_current return Kp * error + Kd * error_dot # 控制器参数 Kp = np.diag([100, 100]) # 比例增益 Kd = np.diag([20, 20]) # 微分增益 # 当前状态和目标状态 q_current = np.array([np.radians(20), np.radians(30)]) q_desired = np.array([np.radians(45), np.radians(60)]) q_dot_current = np.array([0.1, 0.2]) q_dot_desired = np.array([0, 0]) # 计算控制力矩 tau = pd_control(q_current, q_desired, q_dot_current, q_dot_desired, Kp, Kd) print("控制力矩:", tau)

在实际应用中，我们通常会结合动力学模型设计更高级的控制算法，如计算力矩控制或自适应控制。

6. 综合应用：2D连杆机械臂仿真

现在，我们将上述概念整合到一个完整的2D连杆机械臂仿真中。这个仿真将展示如何从数学公式到实际代码实现完整的机器人学应用。

import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation class TwoLinkArm: def __init__(self, l1=1.0, l2=1.0): self.l1 = l1 # 第一连杆长度 self.l2 = l2 # 第二连杆长度 self.q = np.array([0.0, 0.0]) # 关节角度 self.q_dot = np.array([0.0, 0.0]) # 关节速度 def forward_kinematics(self, q=None): """计算正运动学""" if q is None: q = self.q theta1, theta2 = q x = self.l1 * np.cos(theta1) + self.l2 * np.cos(theta1 + theta2) y = self.l1 * np.sin(theta1) + self.l2 * np.sin(theta1 + theta2) return x, y def jacobian(self, q=None): """计算雅可比矩阵""" if q is None: q = self.q theta1, theta2 = q J = np.array([ [-self.l1*np.sin(theta1)-self.l2*np.sin(theta1+theta2), -self.l2*np.sin(theta1+theta2)], [self.l1*np.cos(theta1)+self.l2*np.cos(theta1+theta2), self.l2*np.cos(theta1+theta2)] ]) return J def inverse_kinematics(self, x, y, elbow_up=True): """计算逆运动学""" # 计算第二个关节角度 c2 = (x**2 + y**2 - self.l1**2 - self.l2**2) / (2 * self.l1 * self.l2) s2 = np.sqrt(1 - c2**2) if elbow_up else -np.sqrt(1 - c2**2) theta2 = np.arctan2(s2, c2) # 计算第一个关节角度 k1 = self.l1 + self.l2 * c2 k2 = self.l2 * s2 theta1 = np.arctan2(y, x) - np.arctan2(k2, k1) return np.array([theta1, theta2]) def update(self, tau, dt=0.01): """模拟机械臂动力学""" # 简化的动力学模型 M = np.array([ [self.l1**2 + self.l2**2 + 2*self.l1*self.l2*np.cos(self.q[1]), self.l2**2 + self.l1*self.l2*np.cos(self.q[1])], [self.l2**2 + self.l1*self.l2*np.cos(self.q[1]), self.l2**2] ]) C = np.array([ -self.l1*self.l2*np.sin(self.q[1])*(2*self.q_dot[0]*self.q_dot[1] + self.q_dot[1]**2), self.l1*self.l2*np.sin(self.q[1])*self.q_dot[0]**2 ]) G = np.array([ 9.8*(self.l1*np.cos(self.q[0]) + self.l2*np.cos(self.q[0]+self.q[1])), 9.8*self.l2*np.cos(self.q[0]+self.q[1]) ]) # 计算加速度 q_ddot = np.linalg.inv(M) @ (tau - C - G) # 更新状态 self.q_dot += q_ddot * dt self.q += self.q_dot * dt return self.q, self.q_dot # 创建机械臂实例 arm = TwoLinkArm() # 设置目标位置 x_desired, y_desired = 1.5, 0.5 q_desired = arm.inverse_kinematics(x_desired, y_desired) # 仿真参数 dt = 0.01 sim_time = 5.0 steps = int(sim_time / dt) # 存储仿真数据 history = { 'time': np.zeros(steps), 'q': np.zeros((steps, 2)), 'q_dot': np.zeros((steps, 2)), 'position': np.zeros((steps, 2)) } # 运行仿真 for i in range(steps): # PD控制 tau = pd_control(arm.q, q_desired, arm.q_dot, np.array([0, 0]), Kp, Kd) # 更新机械臂状态 q, q_dot = arm.update(tau, dt) # 记录数据 history['time'][i] = i * dt history['q'][i] = q history['q_dot'][i] = q_dot history['position'][i] = arm.forward_kinematics() # 可视化 fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(12, 5)) # 绘制轨迹 ax1.plot(history['position'][:, 0], history['position'][:, 1], label='末端轨迹') ax1.scatter([0, arm.l1*np.cos(q_desired[0])], [0, arm.l1*np.sin(q_desired[0])], color='red') ax1.scatter([x_desired], [y_desired], color='green', label='目标位置') ax1.set_xlim(-2, 2) ax1.set_ylim(-2, 2) ax1.set_aspect('equal') ax1.set_title('机械臂运动轨迹') ax1.legend() # 绘制关节角度变化 ax2.plot(history['time'], np.degrees(history['q'][:, 0]), label='关节1角度') ax2.plot(history['time'], np.degrees(history['q'][:, 1]), label='关节2角度') ax2.set_xlabel('时间(s)') ax2.set_ylabel('角度(度)') ax2.set_title('关节角度变化') ax2.legend() plt.tight_layout() plt.show()

这个综合示例展示了从数学公式到实际代码的完整流程，包括正逆运动学、雅可比矩阵、动力学模型和控制算法的实现。通过这样的实践，我们能够更深入地理解机器人学中的核心概念。