机器人控制与仿真
这是你最熟悉的领域——运动学、动力学、PID、控制回路。现在只需要把纸上的方程写成Python代码，并在仿真器中验证。你的机械背景在这里是碾压级优势。
1. 刚体变换与位姿表示
你已经知道的
装配体中的每个零件都有一个坐标系。把零件A上的孔位变换到零件B的坐标系——这就是齐次变换。你现在只是用矩阵把它写出来。
齐次变换矩阵——机器人学的通用语言
$T = \begin{bmatrix} R_{3\times 3} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix} \in SE(3)$
一个 4×4 矩阵同时编码了旋转 $R$ 和平移 $\mathbf{t}$。两个变换的组合就是矩阵乘法：$T_{AC} = T_{AB} \cdot T_{BC}$。
旋转的四种表示法
| 表示 | 参数 | 优点 | 缺点 |
|---|---|---|---|
| 旋转矩阵 | 3×3=9 | 无歧义、易组合 | 冗余、需满足正交约束 |
| 欧拉角 | 3 | 直观（roll/pitch/yaw） | 万向节死锁 |
| 四元数 | 4 | 无死锁、平滑插值 | 不直观、需归一化 |
| 轴角 | 3+1 | 直观旋转 | 插值不便 |
🔧 工程连接：UR 机器人的控制接口用轴角（rotation vector），ROS 用四元数，Denavit-Hartenberg 表用旋转矩阵。你必须能在它们之间自由转换。scipy.spatial.transform.Rotation 帮你做这件事。
from scipy.spatial.transform import Rotation as R
import numpy as np
# 欧拉角 → 旋转矩阵
rpy = np.array([0.1, 0.2, 0.3])
rot_mat = R.from_euler('xyz', rpy).as_matrix()
# 四元数 → 旋转矩阵 (x,y,z,w 顺序)
quat = np.array([0, 0, 0.382, 0.924])
rot_mat = R.from_quat(quat).as_matrix()
# 旋转矩阵 → 四元数
quat_back = R.from_matrix(rot_mat).as_quat() # [x,y,z,w]
# 构建齐次变换矩阵
def make_T(rot, trans):
T = np.eye(4)
T[:3,:3] = rot; T[:3,3] = trans
return T
T_world_robot = make_T(R.from_euler('z', 0.5).as_matrix(),
np.array([1.0, 0, 0.5]))
T_robot_camera = make_T(R.from_euler('y', -0.3).as_matrix(),
np.array([0.1, 0, 0.8]))
T_world_camera = T_world_robot @ T_robot_camera
# 逆变换
T_camera_world = np.linalg.inv(T_world_camera)
print(f"相机在世界坐标系中的位姿:\n{T_world_camera.round(3)}")2. DH参数法正运动学
Denavit-Hartenberg (DH) 参数是工业机器人描述连杆几何关系的标准方法。每个关节用4个参数描述——给定关节角度，输出末端位姿。
六轴工业臂完整实现
class SixDOFArm:
"""仿UR5的DH参数正运动学"""
# DH表: [a, alpha, d, theta_offset]
DH = np.array([
[0, np.pi/2, 0.089, 0],
[-0.425, 0, 0, 0],
[-0.392, 0, 0, 0],
[0, np.pi/2, 0.109, 0],
[0, -np.pi/2, 0.095, 0],
[0, 0, 0.082, 0]
])
def dh_T(self, a, alpha, d, theta):
ct,st = np.cos(theta), np.sin(theta)
ca,sa = np.cos(alpha), 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]])
def fk(self, q):
"""正运动学：关节角度→末端位姿，返回所有连杆变换"""
T = np.eye(4); transforms = [T.copy()]
for i, th in enumerate(q):
a,al,d,off = self.DH[i]
T = T @ self.dh_T(a, al, d, th+off)
transforms.append(T.copy())
return transforms
def jacobian(self, q):
"""几何雅可比 J=[Jv; Jω]"""
Ts = self.fk(q); pe = Ts[-1][:3,3]
J = np.zeros((6,6))
for i in range(6):
z = Ts[i][:3,2]; p = Ts[i][:3,3]
J[:3,i] = np.cross(z, pe-p); J[3:,i] = z
return J
arm = SixDOFArm()
q = np.array([0.5, -0.3, 0.2, 0.1, -0.4, 0.3])
pos = arm.fk(q)[-1][:3,3]
print(f"末端位置: {pos.round(3)} m")
# 奇异值分析
J = arm.jacobian(q)
s = np.linalg.svd(J)[1]
print(f"条件数: {s[0]/s[-1]:.1f} (大→接近奇异)")🔧 交互演示：DH参数与机械臂可视化
拖动滑块调整每个关节的DH参数（θ, d, a, α），实时看到机械臂的姿态变化。理解这四个参数如何唯一确定坐标系之间的变换。
3. 数值逆运动学
雅可比伪逆法（Newton-Raphson）
$\Delta\mathbf{q} = J^+ \Delta\mathbf{x}$
其中 $J^+ = J^T(JJ^T)^{-1}$ 是 Moore-Penrose 伪逆。每步计算末端误差，用伪逆映射到关节空间，迭代直到收敛。
def ik_dls(arm, target_pos, target_rot=None, q0=None, max_iter=500):
"""Damped Least Squares IK（比纯伪逆更稳定）"""
q = np.zeros(6) if q0 is None else q0.copy()
for i in range(max_iter):
Ts = arm.fk(q)
p_curr, R_curr = Ts[-1][:3,3], Ts[-1][:3,:3]
p_err = target_pos - p_curr
if target_rot is not None:
R_err = target_rot @ R_curr.T
r_err = R.from_matrix(R_err).as_rotvec()
else: r_err = np.zeros(3)
err = np.concatenate([p_err, r_err])
if np.linalg.norm(err) < 1e-4:
return q, True, i+1
J = arm.jacobian(q)
lam = 0.01 * (1 + np.exp(-i/20)) # 自适应阻尼
dq = np.linalg.solve(J.T@J + lam*np.eye(6), J.T@err)
q += np.clip(dq, -0.5, 0.5)
return q, False, max_iter
q_sol, ok, iters = ik_dls(arm, np.array([0.5,0.2,0.3]))
print(f"IK {'成功' if ok else '失败'} | {iters} 次迭代")实际操作顺序
1. 用 IKFast/解析法求一组初值（如果可用）。2. 用 DLS 精炼。3. 检查关节限位和碰撞。4. 如果失败，换个初始值重试。生产环境中通常做多次随机重启。
4. PID与前馈控制
你已经知道的
你调过伺服电机、温控器、液压系统。PID 的直觉你早已内化。现在只需要把它封装成可复用的类，并理解何时需要前馈补偿。
离散PID
$u_k = K_p e_k + K_i \sum_{j=0}^{k} e_j \Delta t + K_d \frac{e_k - e_{k-1}}{\Delta t}$
class PID:
def __init__(self, kp, ki, kd, dt, i_limit=100):
self.kp,self.ki,self.kd,self.dt = kp, ki, kd, dt
self.i_limit = i_limit
self.reset()
def reset(self): self.integral=0; self.prev_e=0
def __call__(self, sp, pv):
e = sp - pv
self.integral = np.clip(self.integral+e*self.dt, -self.i_limit, self.i_limit)
d = (e - self.prev_e)/self.dt
self.prev_e = e
return self.kp*e + self.ki*self.integral + self.kd*d
🔧 前馈 = 你知道要发生什么，提前补上：重力补偿（$G(q)$）是最基本的前馈。没有它，PID 必须先产生误差才能出力。有了前馈，PID 只处理残余误差，性能提升显著。
5. PyBullet 仿真与阻抗控制
最小仿真示例
import pybullet as p
import pybullet_data
p.connect(p.GUI); p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-9.81)
p.loadURDF("plane.urdf")
robot = p.loadURDF("franka_panda/panda.urdf", useFixedBase=True)
joints = [0,1,2,3,4,5,6]
targets = [0,-0.5,0,-1.5,0,1.0,0.5]
p.setJointMotorControlArray(robot, joints, p.POSITION_CONTROL,
targetPositions=targets, forces=[500]*7)
for _ in range(1000): p.stepSimulation()
p.disconnect()阻抗控制
$F_{ext} = M\ddot{e} + B\dot{e} + Ke$
把机器人变成"虚拟弹簧-阻尼-质量"系统。这是力控和柔顺装配的基础。
class ImpedanceCtrl:
def __init__(self, M, B, K, dt):
self.M,self.B,self.K,self.dt = M, B, K, dt
self.prev_e = np.zeros(6)
def __call__(self, x_des, x_actual):
e = x_des - x_actual
de = (e - self.prev_e)/self.dt
self.prev_e = e.copy()
return self.K@e + self.B@de # 简化忽略加速度项验收实验
- 实现 6-DOF 臂的 DH 正运动学，与 PyBullet 的 FK 结果对比，误差 < 1e-6
- IK 在随机采样的 100 个目标点上成功率 > 95%
- 调一个关节 PID，阶跃响应超调 < 5%，稳态误差 < 0.1°
- 在 PyBullet 中控制 Franka 末端画圆，轨迹误差 < 5mm
- 实现阻抗控制，手推末端能感受到弹性（K=500 N/m），偏差 < 2cm
上一阶段：← 强化学习核心 | 下一阶段：VLA视觉-语言-动作 →