机器人数学基础补充

附录 B：机器人数学基础补充

B.1 线性代数基础

B.1.1 向量运算

向量是机器人学中最基础的数学工具，用于表示位置、速度、力等物理量。

向量定义：

text

复制代码

在三维空间中，向量通常表示为：
v = [v_x, v_y, v_z]^T

基本运算：

运算	公式	几何意义	应用场景
加法	`u + v = [u_x+v_x, u_y+v_y, u_z+v_z]`	向量合成	速度合成、力的叠加
减法	`u - v = [u_x-v_x, u_y-v_y, u_z-v_z]`	向量差	两点间的位移
数乘	`c·v = [c·v_x, c·v_y, c·v_z]`	缩放	速度缩放、力的放大
点积	`u·v = u_xv_x + u_yv_y + u_zv_z`	投影长度	夹角计算、功的计算
叉积	`u×v = [u_yv_z-u_zv_y, u_zv_x-u_xv_z, u_xv_y-u_yv_x]`	法向量	力矩计算、坐标系建立

向量范数：

text

复制代码

L2范数（长度）：‖v‖ = √(v_x² + v_y² + v_z²)
L1范数：‖v‖₁ = |v_x| + |v_y| + |v_z|
无穷范数：‖v‖∞ = max(|v_x|, |v_y|, |v_z|)

单位向量：

text

复制代码

û = v / ‖v‖

向量投影：

text

复制代码

u在v上的投影：proj_v(u) = (u·v) / (v·v) · v

B.1.2 矩阵运算

矩阵用于表示线性变换、旋转、坐标系转换等。

基本定义：

text

复制代码

m×n矩阵A = [a_ij]，其中i=1...m, j=1...n

常用矩阵：

矩阵类型	定义	性质	应用
单位矩阵	I = diag(1,1,...,1)	AI = IA = A	恒等变换
对角矩阵	diag(d₁,d₂,...,dₙ)	仅对角线非零	缩放变换
转置矩阵	(A^T)_ij = A_ji	(AB)^T = B^T A^T	坐标变换
逆矩阵	A⁻¹A = AA⁻¹ = I	(AB)⁻¹ = B⁻¹A⁻¹	反向变换
正交矩阵	R^T = R⁻¹, det(R) = ±1	保持长度	旋转矩阵

基本运算：

运算	公式	条件
加法	(A + B)_ij = A_ij + B_ij	同维数
数乘	(cA)_ij = c·A_ij	-
乘法	(AB)_ij = Σ_k A_ik B_kj	A的列数 = B的行数
行列式	det(A)	方阵
迹	tr(A) = Σ_i A_ii	方阵

特征值与特征向量：

text

复制代码

A·v = λ·v
λ：特征值，v：特征向量

B.1.3 机器人学中的线性代数应用

齐次坐标：

text

复制代码

三维点 (x,y,z) 的齐次坐标表示为 [x, y, z, 1]^T
三维向量 [x,y,z] 的齐次坐标表示为 [x, y, z, 0]^T

齐次变换矩阵：

text

复制代码

T = [R   p
     0   1]  (4×4矩阵)
其中R为3×3旋转矩阵，p为3×1平移向量

坐标系变换：

text

复制代码

点p在坐标系A中的表示 p_A 转换到坐标系B：
p_B = T_A^B · p_A

链式变换：

text

复制代码

T_C^A = T_B^A · T_C^B

B.2 三角函数与几何

B.2.1 基本三角函数

函数	定义	关系
正弦	sinθ = 对边/斜边	sin(-θ) = -sinθ
余弦	cosθ = 邻边/斜边	cos(-θ) = cosθ
正切	tanθ = 对边/邻边	tanθ = sinθ/cosθ
余切	cotθ = 邻边/对边	cotθ = 1/tanθ

重要恒等式：

text

复制代码

sin²θ + cos²θ = 1
sin(α±β) = sinα cosβ ± cosα sinβ
cos(α±β) = cosα cosβ ∓ sinα sinβ
tan(α±β) = (tanα ± tanβ) / (1 ∓ tanα tanβ)

倍角公式：
sin2θ = 2 sinθ cosθ
cos2θ = cos²θ - sin²θ = 2cos²θ - 1 = 1 - 2sin²θ

半角公式：
sin²(θ/2) = (1 - cosθ)/2
cos²(θ/2) = (1 + cosθ)/2

B.2.2 反三角函数

函数	定义域	值域	用途
arcsin	$-1, 1$	$-π/2, π/2$	由比值求角度
arccos	$-1, 1$	$0, π$	由比值求角度
arctan	R	(-π/2, π/2)	由比值求角度
atan2	y, x	(-π, π]	由坐标求角度

atan2(y,x) 的重要性：

text

复制代码

atan2(y,x) 考虑x和y的符号，返回正确象限的角度
θ = atan2(y,x) 满足：
- x>0, y>0: 第一象限 (0<θ<π/2)
- x<0, y>0: 第二象限 (π/2<θ<π)
- x<0, y<0: 第三象限 (-π<θ<-π/2)
- x>0, y<0: 第四象限 (-π/2<θ<0)

B.2.3 几何基础

两点距离：

text

复制代码

d = √[(x₂-x₁)² + (y₂-y₁)² + (z₂-z₁)²]

点与直线距离：

text

复制代码

点P到过点A方向为u的直线的距离：
d = ‖(P-A) - ((P-A)·û)û‖

点与平面距离：

text

复制代码

平面方程：n·x + d = 0
点P到平面距离：|n·P + d| / ‖n‖

三角形内角和：

text

复制代码

α + β + γ = π (180°)

正弦定理：

text

复制代码

a/sinα = b/sinβ = c/sinγ = 2R (R为外接圆半径)

余弦定理：

text

复制代码

c² = a² + b² - 2ab cosγ

B.3 四元数与旋转

B.3.1 四元数定义

四元数是由一个实数部分和三个虚数部分组成的超复数：

text

复制代码

q = q₀ + q₁i + q₂j + q₃k
通常表示为 [q₀, q₁, q₂, q₃]^T

虚数单位性质：

text

复制代码

i² = j² = k² = ijk = -1
ij = k, ji = -k
jk = i, kj = -i
ki = j, ik = -j

单位四元数（用于表示旋转）：

text

复制代码

‖q‖ = √(q₀² + q₁² + q₂² + q₃²) = 1

B.3.2 四元数运算

运算	公式
加法	q + r = $q₀+r₀, q₁+r₁, q₂+r₂, q₃+r₃$
数乘	cq = $c q₀, c q₁, c q₂, c q₃$
乘法	q⊗r = $q₀r₀ - q₁r₁ - q₂r₂ - q₃r₃, q₀r₁ + q₁r₀ + q₂r₃ - q₃r₂, q₀r₂ - q₁r₃ + q₂r₀ + q₃r₁, q₀r₃ + q₁r₂ - q₂r₁ + q₃r₀$ ^T
共轭	q* = $q₀, -q₁, -q₂, -q₃$
范数	‖q‖ = √(q·q*)
逆	q⁻¹ = q* / ‖q‖²（对于单位四元数，q⁻¹ = q*）

B.3.3 四元数与旋转

用单位四元数表示旋转：

text

复制代码

绕单位向量u旋转θ角度的四元数：
q = [cos(θ/2), u_x sin(θ/2), u_y sin(θ/2), u_z sin(θ/2)]

向量旋转：

text

复制代码

三维向量v = [x,y,z] 视为纯四元数 p = [0, x, y, z]
旋转后向量：p' = q ⊗ p ⊗ q⁻¹

四元数与旋转矩阵转换：

四元数 q = $q₀, q₁, q₂, q₃$ 对应的旋转矩阵：

text

复制代码

R = [
    [1-2(q₂²+q₃²),   2(q₁q₂ - q₀q₃), 2(q₁q₃ + q₀q₂)],
    [2(q₁q₂ + q₀q₃), 1-2(q₁²+q₃²),   2(q₂q₃ - q₀q₁)],
    [2(q₁q₃ - q₀q₂), 2(q₂q₃ + q₀q₁), 1-2(q₁²+q₂²)]
]

旋转矩阵转四元数：

text

复制代码

q₀ = ½√(1 + r₁₁ + r₂₂ + r₃₃)
q₁ = (r₃₂ - r₂₃) / (4q₀)
q₂ = (r₁₃ - r₃₁) / (4q₀)
q₃ = (r₂₁ - r₁₂) / (4q₀)

B.3.4 四元数与欧拉角转换

四元数转欧拉角（ZYX顺序）：

text

复制代码

roll  = atan2(2(q₀q₁ + q₂q₃), 1 - 2(q₁² + q₂²))
pitch = asin(2(q₀q₂ - q₃q₁))
yaw   = atan2(2(q₀q₃ + q₁q₂), 1 - 2(q₂² + q₃²))

欧拉角转四元数（ZYX顺序）：

text

复制代码

q₀ = cos(roll/2) cos(pitch/2) cos(yaw/2) + sin(roll/2) sin(pitch/2) sin(yaw/2)
q₁ = sin(roll/2) cos(pitch/2) cos(yaw/2) - cos(roll/2) sin(pitch/2) sin(yaw/2)
q₂ = cos(roll/2) sin(pitch/2) cos(yaw/2) + sin(roll/2) cos(pitch/2) sin(yaw/2)
q₃ = cos(roll/2) cos(pitch/2) sin(yaw/2) - sin(roll/2) sin(pitch/2) cos(yaw/2)

B.3.5 四元数的优点与缺点

优点	缺点
无万向锁问题	不够直观
计算效率高（仅需乘加）	需要归一化
插值平滑（球面线性插值）	转换到欧拉角复杂
存储紧凑（4个浮点数）	理解困难
易于组合旋转	存在双值覆盖（q和-q表示同一旋转）

B.4 刚体运动学

B.4.1 位置与姿态

刚体在空间中的位姿由位置和姿态共同描述：

位置：用向量 p = $x, y, z$ ^T 表示
姿态：用旋转矩阵 R 或四元数 q 表示

齐次变换矩阵（4×4）：

text

复制代码

T = [R  p
     0  1]

变换的性质：

text

复制代码

T₁·T₂ 表示先进行T₂变换，再进行T₁变换
T⁻¹ = [R^T  -R^T p; 0 1]

B.4.2 旋转的表示方法对比

表示方法	参数数量	优点	缺点	应用场景
旋转矩阵	9	无奇异，易组合	冗余，计算量大	理论推导
欧拉角	3	直观	万向锁	人机交互
旋转向量	3	紧凑	需要处理周期性	姿态更新
四元数	4	无奇异，高效	不够直观	实时计算
轴角	4	直观	计算复杂	初始化

B.4.3 刚体运动

速度关系：

text

复制代码

v_B = v_A + ω × r_AB   (点的速度)
其中v_A、v_B为A、B点的速度，ω为角速度

加速度关系：

text

复制代码

a_B = a_A + α × r_AB + ω × (ω × r_AB)
其中α为角加速度

刚体的动能：

text

复制代码

T = ½ m v_c^2 + ½ ω^T I_c ω
其中v_c为质心速度，I_c为刚体关于质心的惯性张量

B.5 机器人运动学

B.5.1 正运动学

D-H参数法是建立机器人运动学模型的经典方法。

D-H参数定义：

参数	符号	物理意义
关节角	θ_i	绕z_{i-1}轴从x_{i-1}到x_i的转角
连杆偏移	d_i	沿z_{i-1}轴从x_{i-1}到x_i的距离
连杆长度	a_i	沿x_i轴从z_{i-1}到z_i的距离
连杆扭角	α_i	绕x_i轴从z_{i-1}到z_i的转角

相邻坐标系变换矩阵：

text

复制代码

T_i^{i-1} = Rot_z(θ_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(α_i)

= [cosθ_i   -sinθ_i cosα_i    sinθ_i sinα_i    a_i cosθ_i
   sinθ_i    cosθ_i cosα_i   -cosθ_i sinα_i    a_i sinθ_i
   0         sinα_i           cosα_i            d_i
   0         0                0                 1]

末端位姿：

text

复制代码

T_n^0 = T_1^0 · T_2^1 · ... · T_n^{n-1}

B.5.2 逆运动学

逆运动学问题是求解非线性方程组：

text

复制代码

T_n^0(q) = T_desired

解的存在性：期望位姿必须在机器人的工作空间内。

解的多重性：通常存在多组解，需要根据约束选择。

解析解存在的条件（Pieper准则）：

三个相邻关节轴交于一点（如球形腕）
三个相邻关节轴平行

数值解法（用于一般机器人）：

text

复制代码

使用雅可比矩阵迭代：q_{k+1} = q_k + J(q_k)⁺ · e_k
其中e_k为当前位姿与目标位姿的误差

B.5.3 雅可比矩阵

定义：雅可比矩阵J建立了关节速度与末端速度的关系：

text

复制代码

v = J(q) · q̇
其中v = [v_x, v_y, v_z, ω_x, ω_y, ω_z]^T 为末端速度

构造方法：

线速度部分：对于旋转关节，J_v的第i列为 z_{i-1} × (p_n - p_{i-1})
角速度部分：对于旋转关节，J_ω的第i列为 z_{i-1}

雅可比矩阵的应用：

text

复制代码

逆速度学：q̇ = J⁺ · v_desired
静力学：τ = J^T · F
奇异性分析：det(J) = 0 时处于奇异位形

B.5.4 常见机器人模型

两连杆平面机械臂：

text

复制代码

正运动学：
x = L₁ cosθ₁ + L₂ cos(θ₁+θ₂)
y = L₁ sinθ₁ + L₂ sin(θ₁+θ₂)

雅可比矩阵：
J = [ -L₁ sinθ₁ - L₂ sin(θ₁+θ₂),  -L₂ sin(θ₁+θ₂)
       L₁ cosθ₁ + L₂ cos(θ₁+θ₂),   L₂ cos(θ₁+θ₂) ]

SCARA机器人：

text

复制代码

特点：三个旋转关节（轴平行）+ 一个平移关节
常用于：装配、拾取放置任务

六自由度串联机器人：

text

复制代码

典型结构：三个定位关节 + 三个定向关节（球形腕）
存在解析解，广泛应用于工业机器人

B.6 机器人动力学

B.6.1 牛顿-欧拉方程

牛顿方程（平动）：

text

复制代码

F = m·a

欧拉方程（转动）：

text

复制代码

τ = I·α + ω × (I·ω)
其中I为惯性张量

B.6.2 拉格朗日方法

拉格朗日函数：

text

复制代码

L = T - V
T为系统动能，V为势能

拉格朗日方程：

text

复制代码

d/dt (∂L/∂q̇) - ∂L/∂q = τ

串联机器人动力学方程（标准形式）：

text

复制代码

M(q)̈q + C(q,˙q)˙q + g(q) = τ
其中：
M(q)：惯性矩阵
C(q,˙q)：科里奥利力和离心力项
g(q)：重力项

B.6.3 动力学参数

参数	符号	物理意义
质量	m	刚体的质量
质心	c	刚体的质心位置
惯性张量	I	描述质量分布
惯性矩	I_xx, I_yy, I_zz	绕坐标轴的惯性
惯性积	I_xy, I_yz, I_zx	坐标轴间的耦合

惯性张量矩阵：

text

复制代码

I = [I_xx  -I_xy  -I_xz
     -I_xy  I_yy  -I_yz
     -I_xz -I_yz   I_zz]

B.6.4 动力学控制

计算力矩控制：

text

复制代码

τ = M(q) (̈q_d + K_d ė + K_p e) + C(q,˙q)˙q + g(q)

阻抗控制：

text

复制代码

F = K (x_d - x) + D (v_d - v) + M (a_d - a)

力/位混合控制 ：

在选择矩阵S指定的方向进行位置控制，在正交方向进行力控制。

B.7 坐标系与变换

B.7.1 常用坐标系

坐标系	定义	用途
世界坐标系	固定于环境	全局参考
机器人基坐标系	固定于机器人基座	机器人参考
工具坐标系	固定于末端执行器	操作参考
工件坐标系	固定于工件	任务参考
相机坐标系	固定于相机	视觉感知
imu坐标系	固定于IMU	姿态感知

B.7.2 坐标变换

旋转矩阵的性质：

text

复制代码

R ∈ SO(3)，满足：
R^T R = I
det(R) = 1
R⁻¹ = R^T

欧拉角旋转顺序：

text

复制代码

常用ZYX顺序（偏航-俯仰-滚转）：
R = R_z(ψ) · R_y(θ) · R_x(φ)

RPY角（横滚-俯仰-偏航）：

text

复制代码

roll (φ)   : 绕X轴旋转
pitch (θ)  : 绕Y轴旋转
yaw (ψ)    : 绕Z轴旋转

变换的复合：

text

复制代码

给定点p在坐标系C中的表示p_C，求在坐标系A中的表示：
p_A = T_B^A · T_C^B · p_C

B.7.3 手眼标定

眼在手外（相机固定）：

text

复制代码

相机坐标系C到机器人基坐标系B的变换T_C^B为常数

眼在手上（相机装在机器人上）：

text

复制代码

相机坐标系C到工具坐标系E的变换T_C^E为常数

标定方程：

text

复制代码

AX = XB
其中A为机器人运动，B为视觉观测，X为待求的手眼变换

B.8 概率论与状态估计

B.8.1 概率基础

随机变量：

text

复制代码

期望：E[X] = ∫ x·p(x) dx
方差：Var(X) = E[(X - E[X])²]
协方差：Cov(X,Y) = E[(X - E[X])(Y - E[Y])]

高斯分布（正态分布）：

text

复制代码

概率密度函数：p(x) = (1/√(2πσ²)) exp(-(x-μ)²/(2σ²))
多变量高斯：p(x) = (1/√((2π)ⁿ|Σ|)) exp(-½ (x-μ)ᵀΣ⁻¹(x-μ))

B.8.2 贝叶斯滤波

贝叶斯公式：

text

复制代码

p(x|z) = p(z|x) p(x) / p(z)
后验 = 似然 × 先验 / 证据

递归贝叶斯滤波：

text

复制代码

预测：p(x_k | z_{1:k-1}) = ∫ p(x_k | x_{k-1}) p(x_{k-1} | z_{1:k-1}) dx_{k-1}
更新：p(x_k | z_{1:k}) = p(z_k | x_k) p(x_k | z_{1:k-1}) / p(z_k | z_{1:k-1})

B.8.3 卡尔曼滤波

线性系统模型：

text

复制代码

状态方程：x_k = F_k x_{k-1} + B_k u_k + w_k
观测方程：z_k = H_k x_k + v_k
其中 w_k ~ N(0, Q_k), v_k ~ N(0, R_k)

预测步骤：

text

复制代码

x̂_{k|k-1} = F_k x̂_{k-1|k-1} + B_k u_k
P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k

更新步骤：

text

复制代码

ỹ_k = z_k - H_k x̂_{k|k-1}
S_k = H_k P_{k|k-1} H_k^T + R_k
K_k = P_{k|k-1} H_k^T S_k⁻¹
x̂_{k|k} = x̂_{k|k-1} + K_k ỹ_k
P_{k|k} = (I - K_k H_k) P_{k|k-1}

B.8.4 扩展卡尔曼滤波

非线性系统：

text

复制代码

x_k = f(x_{k-1}, u_k) + w_k
z_k = h(x_k) + v_k

线性化：

text

复制代码

F_k = ∂f/∂x |_{x̂_{k-1|k-1}}
H_k = ∂h/∂x |_{x̂_{k|k-1}}

B.8.5 粒子滤波

核心思想：用加权样本（粒子）近似后验分布。

重要性采样：

text

复制代码

x_kⁱ ∼ q(x_k | x_{k-1}ⁱ, z_k)
w_kⁱ = w_{k-1}ⁱ · p(z_k | x_kⁱ) p(x_kⁱ | x_{k-1}ⁱ) / q(x_kⁱ | x_{k-1}ⁱ, z_k)

重采样 ：

去除低权重粒子，复制高权重粒子，解决粒子退化问题。

B.9 优化理论

B.9.1 最小二乘

线性最小二乘：

text

复制代码

最小化 ‖Ax - b‖²
解：x̂ = (A^T A)⁻¹ A^T b

加权最小二乘：

text

复制代码

最小化 ‖W(Ax - b)‖²
解：x̂ = (A^T W^T W A)⁻¹ A^T W^T W b

B.9.2 非线性优化

高斯-牛顿法：

text

复制代码

对于 f(x) = ½ Σ r_i(x)²
迭代：x_{k+1} = x_k - (J^T J)⁻¹ J^T r
其中J为雅可比矩阵

列文伯格-马夸尔特法：

text

复制代码

x_{k+1} = x_k - (J^T J + λ I)⁻¹ J^T r
λ为阻尼因子，在梯度下降和高斯-牛顿之间切换

B.9.3 图优化

在SLAM中的应用：

text

复制代码

误差函数：e_ij(x_i, x_j) = z_ij - h(x_i, x_j)
目标函数：F(x) = Σ e_ij^T Ω_ij e_ij
求解：x* = argmin F(x)

B.10 常用数学公式速查

B.10.1 三角恒等式

text

复制代码

sin(α ± β) = sinα cosβ ± cosα sinβ
cos(α ± β) = cosα cosβ ∓ sinα sinβ
tan(α ± β) = (tanα ± tanβ) / (1 ∓ tanα tanβ)

sin2α = 2 sinα cosα
cos2α = cos²α - sin²α = 2 cos²α - 1 = 1 - 2 sin²α
tan2α = 2 tanα / (1 - tan²α)

sin²α + cos²α = 1
1 + tan²α = sec²α
1 + cot²α = csc²α

B.10.2 向量恒等式

text

复制代码

a × (b × c) = b(a·c) - c(a·b)
(a × b) × c = b(a·c) - a(b·c)
(a × b)·c = a·(b × c)
(a × b) × (c × d) = c(a·(b × d)) - d(a·(b × c))

B.10.3 矩阵恒等式

text

复制代码

(A⁻¹)ᵀ = (Aᵀ)⁻¹
(AB)⁻¹ = B⁻¹ A⁻¹
(AB)ᵀ = Bᵀ Aᵀ
(A + B)ᵀ = Aᵀ + Bᵀ
(A⁻¹ + B⁻¹)⁻¹ = A(A + B)⁻¹B

矩阵求逆引理（Woodbury公式）：

text

复制代码

(A + UCV)⁻¹ = A⁻¹ - A⁻¹U(C⁻¹ + VA⁻¹U)⁻¹VA⁻¹

B.10.4 导数公式

标量对向量的导数：

text

复制代码

∂(a·x)/∂x = a
∂(x·x)/∂x = 2x
∂(x^T A x)/∂x = (A + A^T)x

向量对向量的导数（雅可比矩阵）：

text

复制代码

∂(Ax)/∂x = A
∂(x^T A)/∂x = A^T

B.11 机器人数学库推荐

库名	语言	功能	特点
Eigen	C++	线性代数	高效、模板化
Armadillo	C++	线性代数	MATLAB风格
Sophus	C++	李群李代数	Lie groups
Ceres Solver	C++	非线性优化	图优化、SLAM
g2o	C++	图优化	SLAM专用
GTSAM	C++	因子图优化	贝叶斯推理
OpenCV	C++/Python	视觉+几何	相机模型、变换
NumPy/SciPy	Python	科学计算	快速原型
SymPy	Python	符号计算	公式推导
Pinocchio	C++/Python	动力学	刚体动力学

本附录持续更新。建议读者在实际开发中根据需要查阅相关数学参考资料，建立自己的数学知识体系。