数学与编程基础

1. 向量基础与空间直觉

为什么向量是机器人学的"母语"

机器人学的每一个核心概念——位置、姿态、速度、力、雅可比——都是用向量表达的。理解向量的本质，就理解了机器人学的一半。

向量的两种视角

• 几何视角：有向线段，有大小和方向
• 代数视角：有序数组 $\mathbf{v} = (v_1, v_2, \ldots, v_n) \in \mathbb{R}^n$

点积（内积）——最重要的向量运算

点积有两种等价定义，这正是它强大的原因：

代数定义告诉我们怎么算，几何定义告诉我们什么意思。

在机器人学中，点积无处不在：

• 判断方向：$\mathbf{a} \cdot \mathbf{b} > 0 \Rightarrow$ 夹角 < 90°，方向大致一致
• 计算投影：$\text{proj}_{\mathbf{b}}(\mathbf{a}) = \frac{\mathbf{a} \cdot \mathbf{b}}{\|\mathbf{b}\|^2} \mathbf{b}$
• 计算功：$W = \mathbf{F} \cdot \mathbf{d}$，力沿位移方向做功
• 夹爪接近方向：判断夹爪是否对准物体表面法线

叉积（外积）——三维专属

结果是垂直于两个向量所在平面的向量，方向由右手定则确定。大小 $\|\mathbf{a} \times \mathbf{b}\| = \|\mathbf{a}\|\|\mathbf{b}\|\sin\theta$，正是平行四边形的面积。

向量范数（长度）

第一个代码实验

import numpy as np

# ===== 向量的表示 =====
pos = np.array([0.5, 0.3, 0.8])      # 末端位置 (m)
target = np.array([0.7, 0.1, 0.9])   # 目标位置 (m)

# ===== 基本运算 =====
dist = np.linalg.norm(pos - target)       # 欧几里得距离
direction = (target - pos) / dist           # 单位方向向量
dot = np.dot(pos, target)                  # 点积

# ===== 力与力矩 =====
F = np.array([5.0, 0.0, 0.0])          # 末端力 (N)
r = np.array([0.3, 0.0, 0.0])          # 力臂 (m)
torque = np.cross(r, F)                   # 力矩 = r × F

print(f"距离: {dist:.4f} m")
print(f"方向向量: {direction}")
print(f"力矩: {torque} Nm")

2. 矩阵运算与线性变换

矩阵乘法：组合变换

矩阵乘法对应变换的组合：先做B变换再做A变换，等价于做 $C = AB$ 这一次变换。但要注意 $AB \neq BA$——变换的顺序不能随意交换！

旋转矩阵：机器人学的"身份证"

绕 $z$ 轴旋转 $\theta$ 角度的旋转矩阵：

旋转矩阵有两个关键性质：

• 正交性：$R^T R = I$，$R^{-1} = R^T$（求逆只需转置！）
• 行列式为1：$\det(R) = 1$（保证是纯旋转，不含反射）

齐次变换：同时处理旋转和平移

这是机器人学最重要的数据结构。一个 4×4 矩阵同时编码了旋转和平移：

import numpy as np

def make_transform(rotation, translation):
    """构建齐次变换矩阵"""
    T = np.eye(4)
    T[0:3, 0:3] = rotation
    T[0:3, 3] = translation
    return T

# 构建：绕z轴旋转30°，平移(1, 0, 0.5)
theta = np.radians(30)
R = np.array([
    [np.cos(theta), -np.sin(theta), 0],
    [np.sin(theta),  np.cos(theta), 0],
    [0,            0,            1]
])
t = np.array([1.0, 0.0, 0.5])
T = make_transform(R, t)

# 链式变换：world → robot_base → camera
T_world_camera = T_world_robot @ T_robot_camera # 矩阵乘法 = 变换组合

# 变换一个点：将相机坐标系下的点变换到世界坐标系
point_cam = np.array([0.2, 0.1, 0.3, 1.0])  # 齐次坐标
point_world = T_world_camera @ point_cam
print(f"世界坐标: {point_world[:3]}")

矩阵的迹与行列式

$\text{tr}(A) = \sum A_{ii}$（主对角线元素之和），在旋转矩阵中 $\text{tr}(R) = 1 + 2\cos\theta$，可以反推旋转角度。

$\det(A)$ 的物理意义是变换的体积缩放因子。$\det(R)=1$ 保证旋转不改变体积。

3. 特征值与奇异值分解

特征值分解

$\lambda$ 是特征值，$\mathbf{v}$ 是对应的特征向量。矩阵 $A$ 作用在 $\mathbf{v}$ 上，只是把它拉伸/压缩了 $\lambda$ 倍，方向不变。

机器人学中的特征值：

• 主成分分析 (PCA)：用协方差矩阵的特征向量找数据的主要变化方向
• 稳定性分析：动力学系统的雅可比矩阵特征值实部决定系统是否稳定
• 可操作性椭球：$JJ^T$ 的特征值描述机械臂各方向的"灵活度"

奇异值分解 (SVD)

SVD 是"任何矩阵都能对角化"的保证。$\Sigma$ 的对角元素（奇异值）描述了矩阵在各个方向上的"放大倍数"。

import numpy as np

# 机械臂在某姿态下的雅可比矩阵
J = np.array([[0.5, -0.2, 0.1],
              [0.3,  0.8, -0.1],
              [0.0,  0.1,  0.9]])

U, s, Vt = np.linalg.svd(J)

print(f"奇异值: {s}")  # 沿各主轴的"运动能力"
print(f"条件数: {s[0]/s[-1]:.1f}")  # 大→接近奇异
print(f"可操作度: {np.prod(s):.4f}")  # 综合运动能力

4. 微积分与优化基础

梯度——多变量函数的"斜率"

梯度指向函数值增长最快的方向。在机器学习中，梯度下降就是沿着负梯度方向走，逐步走到函数的最小值点。

雅可比矩阵——向量函数的"导数"

雅可比是机器人学中最核心的矩阵之一：

• $\dot{\mathbf{x}} = J(\mathbf{q})\dot{\mathbf{q}}$：关节速度 → 末端速度
• $\boldsymbol{\tau} = J^T(\mathbf{q}) \mathbf{F}$：末端力 → 关节力矩

# 数值雅可比：用有限差分近似
def numerical_jacobian(fk_fn, q, eps=1e-6):
    """用有限差分计算雅可比"""
    n_joints = len(q)
    f0 = fk_fn(q)
    J = np.zeros((len(f0), n_joints))
    for i in range(n_joints):
        q_perturb = q.copy()
        q_perturb[i] += eps
        J[:, i] = (fk_fn(q_perturb) - f0) / eps
    return J

梯度下降 vs 牛顿法

IK 中的 Newton-Raphson 方法就是牛顿法的应用：$\Delta\mathbf{q} = J^+ \Delta\mathbf{x}$，用雅可比伪逆替代 Hessian 的逆。

5. 概率统计与贝叶斯推断——机器人学的决策语言

概率分布：随机变量的数学描述

贝叶斯定理：从先验到后验

这是机器人学中最重要的公式。它告诉你：拿到新证据(E)后，你的信念(H)应该如何更新。

import numpy as np

# 机器人接触检测的贝叶斯更新
# 先验：有10%概率在接触某物体
P_contact = 0.10

# 传感器：检测到力 > 阈值
# P(传感器报警 | 接触) = 0.95 (检测率)
# P(传感器报警 | 未接触) = 0.02 (误报率)
P_alarm_given_contact = 0.95
P_alarm_given_no_contact = 0.02

# 传感器报警后，实际接触的概率？
P_alarm = (P_alarm_given_contact * P_contact +
           P_alarm_given_no_contact * (1 - P_contact))

P_contact_given_alarm = (P_alarm_given_contact * P_contact) / P_alarm

print(f"报警后接触概率: {P_contact_given_alarm:.2%}")

期望与方差

期望 = "平均结果" = 长期平均奖励。方差 = "不确定性大小" = 策略的稳定程度。

协方差矩阵：描述多维不确定性

import numpy as np

# ❌ 坏习惯：Python 循环
result = []
for i in range(1000000):
    result.append(np.sin(i * 0.01))

# ✅ 好习惯：NumPy 向量化
x = np.arange(0, 10000, 0.01)
result = np.sin(x)  # 一行，快 50 倍

import matplotlib.pyplot as plt

# 可视化机械臂工作空间
angles = np.linspace(-np.pi, np.pi, 200)
x = np.cos(angles) + 0.8 * np.cos(angles * 2)
y = np.sin(angles) + 0.8 * np.sin(angles * 2)

plt.figure(figsize=(8, 8))
plt.plot(x, y, 'b-', linewidth=2)
plt.axis('equal')
plt.grid(True, alpha=0.3)
plt.title('2-DOF 机械臂工作空间边界')
plt.xlabel('X (m)'); plt.ylabel('Y (m)')

在机器人学中，协方差矩阵无处不在：SLAM中的位姿不确定性、Kalman滤波的误差传播、抓取时的接触不确定性。对角线是各维度的方差，非对角线是两个维度之间的"联动"。