机器人学导论核心公式实战从数学推导到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, l11, l21): 计算2自由度平面机械臂的雅可比矩阵 J np.zeros((2, 2)) J[0, 0] -l1*np.sin(theta1) - l2*np.sin(theta1theta2) J[0, 1] -l2*np.sin(theta1theta2) J[1, 0] l1*np.cos(theta1) l2*np.cos(theta1theta2) J[1, 1] l2*np.cos(theta1theta2) 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, l11.0, l21.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, qNone): 计算正运动学 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, qNone): 计算雅可比矩阵 if q is None: q self.q theta1, theta2 q J np.array([ [-self.l1*np.sin(theta1)-self.l2*np.sin(theta1theta2), -self.l2*np.sin(theta1theta2)], [self.l1*np.cos(theta1)self.l2*np.cos(theta1theta2), self.l2*np.cos(theta1theta2)] ]) return J def inverse_kinematics(self, x, y, elbow_upTrue): 计算逆运动学 # 计算第二个关节角度 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, dt0.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])], colorred) ax1.scatter([x_desired], [y_desired], colorgreen, 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()这个综合示例展示了从数学公式到实际代码的完整流程包括正逆运动学、雅可比矩阵、动力学模型和控制算法的实现。通过这样的实践我们能够更深入地理解机器人学中的核心概念。