进阶 · 4-6周

机器人控制与仿真

这是你最熟悉的领域——运动学、动力学、PID、控制回路。现在只需要把纸上的方程写成Python代码，并在仿真器中验证。你的机械背景在这里是碾压级优势。

4-6 周

5 个章节

10+ 代码示例

5 个验收实验

本阶段目录

刚体变换与位姿表示
DH参数法正运动学
数值逆运动学
PID与前馈控制
PyBullet仿真与阻抗控制
机器人动力学
状态估计：Kalman滤波

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, α），实时看到机械臂的姿态变化。理解这四个参数如何唯一确定坐标系之间的变换。

class="animate-in">

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视觉-语言-动作 →

拓展资源：机器人控制

GitHub 仓库

bulletphysics/bullet3 — PyBullet 物理引擎
petercorke/robotics-toolbox-python — Python 机器人工具箱
ros/ros2_documentation — ROS 2 官方文档
ros-planning/moveit2 — 运动规划框架
NVIDIA/IsaacGymEnvs — GPU 并行机器人仿真

视频课程

Modern Robotics Video Series — Lynch & Park 配套视频
Articulated Robotics — ROS 2 实战教程

必读教材

Modern Robotics (Lynch & Park) — 免费在线版，含代码示例
Robotic Manipulation (Russ Tedrake) — MIT 操作课程笔记

机器人动力学：从运动学到力

对照已有知识

你做过机构动力学分析：关节驱动力如何转化为机构运动。机器人学中用的是Euler-Lagrange形式：

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

这个公式三个分量分别对应：

$M(q)\ddot{q}$ — 惯性项：加速需要多大扭矩（和关节位置有关！）
$C(q,\dot{q})\dot{q}$ — 科氏力/离心力项：高速运动时的耦合效应
$G(q)$ — 重力项：关节位置不同，自身重量产生的力矩不同

在RL和控制器中的意义

# 用PyBullet获取动力学参数
import pybullet as p

# 获取7-DOF臂的质量矩阵和重力向量
M = p.calculateMassMatrix(robot_id, joint_positions)
G = p.calculateInverseDynamics(robot_id, q, qd, qdd=[0]*7)

# 重力补偿 → PID只处理动态误差，性能大幅提升
torque = pid_output + G  # 这是控制工程中的标准模式

🔧 工程直觉：动力学模型 = 前馈控制的数学基础。你不需要手推Euler-Lagrange方程——PyBullet和RBDL可以自动计算。但你需要理解每个物理量的含义，才能在调试时知道是"惯量匹配问题"还是"重力补偿不够"。

状态估计：Kalman滤波入门

为什么需要状态估计

传感器永远不准。编码器有量化误差，IMU有漂移，摄像头有噪声。Kalman滤波是最优地融合多个噪声传感器的标准方法——它在每个机器人系统中都不可或缺。

Kalman滤波的两个步骤

预测：$\hat{x}_{k|k-1} = F \hat{x}_{k-1} \qquad P_{k|k-1} = F P_{k-1} F^T + Q$
更新：$K = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1}$
$\hat{x}_k = \hat{x}_{k|k-1} + K(y_k - H\hat{x}_{k|k-1})$

import numpy as np

class KalmanFilter1D:
    """一维Kalman滤波——入门理解"""
    def __init__(self, init_x=0, init_P=1, Q=0.01, R=0.1):
        self.x = init_x    # 状态估计
        self.P = init_P    # 估计的不确定性
        self.Q = Q         # 过程噪声（模型有多不准确）
        self.R = R         # 测量噪声（传感器有多不准确）

    def update(self, measurement):
        # 预测：模型认为状态没变，但不确定性增加了
        self.P += self.Q

        # 更新：用测量修正预测
        K = self.P / (self.P + self.R)  # Kalman增益
        self.x += K * (measurement - self.x)
        self.P *= (1 - K)
        return self.x

# 模拟：真值=5.0，传感器噪声std=0.3
kf = KalmanFilter1D(R=0.09)
estimates = []; measurements = []
for _ in range(20):
    meas = 5.0 + np.random.randn() * 0.3
    est = kf.update(meas)
    measurements.append(meas); estimates.append(est)

print(f"传感器RMSE: {np.std(measurements):.3f}")
print(f"Kalman RMSE: {np.std(np.array(estimates)-5.0):.3f}")
print(f"改进: {100*(1-np.std(np.array(estimates)-5.0)/np.std(measurements)):.0f}%")

🔧 工程直觉：Kalman增益 K 在 0 和 1 之间。K≈0 → 更信任模型预测（传感器不可靠时）；K≈1 → 更信任测量（传感器准确时）。这个权重是自动计算的——根据你对模型(Q)和传感器(R)的不确定度设定。

上一阶段：强化学习核心 VLA视觉-语言-动作