01 数学与理论基础
七轴机械臂算法的"地基"：所有后面内容（IK、雅可比、阻抗、双臂内力）都建立在这套数学上。
目标：能闭着眼睛在代码里写出 旋转、四元数、指数映射、伪逆、卡尔曼滤波。
配套代码：
code/01_examples.py，11 个示例全部可运行（python3 code/01_examples.py）。
1. 三维姿态表示
1.1 旋转矩阵 R ∈ S O ( 3 ) R \in SO(3) R∈SO(3) ------ "姿态的字典"
通俗理解 ：旋转矩阵就是一列"在基座系里表示出来的新坐标轴方向"。 R R R 的第 1 列是旋转后新坐标系 x x x 轴在基座系里的坐标，第 2、3 列同理。你不需要背公式，只需要知道：拿到矩阵，就能把任何点旋转过去。
-
三个性质（前两个是工程上的照妖镜）：
- 正交： R T R = I R^T R = I RTR=I
- 行列式： det ( R ) = + 1 \det(R) = +1 det(R)=+1（= − 1 -1 −1 就是镜像，物理上不可能）
- 逆 = 转置： R − 1 = R T R^{-1} = R^T R−1=RT
-
三轴基本旋转（右手系，绕谁转谁不动）：
R x ( θ ) = 1 0 0 0 cos θ − sin θ 0 sin θ cos θ , R y ( θ ) = cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ , R z ( θ ) = cos θ − sin θ 0 sin θ cos θ 0 0 0 1 R_x(\theta)=\begin{bmatrix}1&0&0\\0&\cos\theta&-\sin\theta\\0&\sin\theta&\cos\theta\end{bmatrix},\quad R_y(\theta)=\begin{bmatrix}\cos\theta&0&\sin\theta\\0&1&0\\-\sin\theta&0&\cos\theta\end{bmatrix},\quad R_z(\theta)=\begin{bmatrix}\cos\theta&-\sin\theta&0\\\sin\theta&\cos\theta&0\\0&0&1\end{bmatrix} Rx(θ)= 1000cosθsinθ0−sinθcosθ ,Ry(θ)= cosθ0−sinθ010sinθ0cosθ ,Rz(θ)= cosθsinθ0−sinθcosθ0001
真实数据代入（六轴） ：末端绕基座 Z Z Z 轴转 30 ° 30° 30°， cos 30 ° ≈ 0.866 , sin 30 ° = 0.5 \cos30°≈0.866,\ \sin30°=0.5 cos30°≈0.866, sin30°=0.5：
R z ( 30 ° ) = 0.866 − 0.5 0 0.5 0.866 0 0 0 1 R_z(30°) = \begin{bmatrix}0.866&-0.5&0\\0.5&0.866&0\\0&0&1\end{bmatrix} Rz(30°)= 0.8660.50−0.50.8660001
把基座系下的点 p = ( 1 , 0 , 0 ) p=(1,0,0) p=(1,0,0) 变换过去： p ′ = R p = ( 0.866 , 0.5 , 0 ) p' = Rp = (0.866,\ 0.5,\ 0) p′=Rp=(0.866, 0.5, 0)，长度仍是 1 1 1（旋转不改变长度）。代码示例1验证 R T R = I R^TR=I RTR=I 误差为 0 0 0、 det = + 1 \det=+1 det=+1。
在真实控制系统里 ：每个 link 的坐标系方向就是一张旋转矩阵；TF 话题里的 geometry_msgs/Transform.rotation 是四元数，但内部换算成矩阵做乘法（四元数只是"给 TF 通信用的压缩格式"）。
工程坑
坑 现象 解法 数值漂移 连乘几千次后 R T R ≠ I R^TR \ne I RTR=I（矩阵"长歪了"） 定期用 SVD 正交化： R = U V T R = UV^T R=UVT 角度单位混用 该用 rad写了deg→ 末端转到天上去了统一 deg2rad，在驱动接口层检查手/眼系混用 相机系的矩阵当成基座系用 变换永远写清"谁相对谁"： R a b R_a^b Rab
1.2 欧拉角 ------ "会骗人的三个数"
R = R z ( ψ ) R y ( θ ) R x ( ϕ ) ( ZYX / roll-pitch-yaw ) R = R_z(\psi)\,R_y(\theta)\,R_x(\phi) \quad (\text{ZYX / roll-pitch-yaw}) R=Rz(ψ)Ry(θ)Rx(ϕ)(ZYX / roll-pitch-yaw)
先绕自身 x x x 转 ϕ \phi ϕ（roll），再绕新的 y y y 转 θ \theta θ（pitch），再绕新的 z z z 转 ψ \psi ψ（yaw）。顺序和内旋/外旋约定必须固定，不同约定结果完全不同。
真实数据代入（六轴） ：roll=30°, pitch=45°, yaw=60° 合成（代码示例2）：
R = R z ( 60 ° ) R y ( 45 ° ) R x ( 30 ° ) = 0.354 − 0.573 0.739 0.612 0.739 0.280 − 0.707 0.354 0.612 R = R_z(60°)R_y(45°)R_x(30°) = \begin{bmatrix}0.354&-0.573&0.739\\0.612&0.739&0.280\\-0.707&0.354&0.612\end{bmatrix} R=Rz(60°)Ry(45°)Rx(30°)= 0.3540.612−0.707−0.5730.7390.3540.7390.2800.612
万向锁（为什么工程上不用欧拉角做姿态插值） ：当 p i t c h = 90 ° pitch=90° pitch=90° 时，roll 和 yaw 的旋转轴重合 了------丢了一个自由度。示例2 验证：R(roll=30,yaw=60) 与 R(roll=0,yaw=90) 是同一个姿态（绕同一轴转了等价角度）。表现就是姿态插值时末端会"猛甩一下"、绕个大圈。
在真实控制系统里 ：示教器上显示欧拉角（人类友好）；但控制内部从不用欧拉角插值，一律用四元数或轴角。
1.3 四元数 q = ( w , x , y , z ) q = (w, x, y, z) q=(w,x,y,z) ------ "姿态插值的正规军"
- 单位四元数 ∣ q ∣ = 1 |q|=1 ∣q∣=1；每个姿态有两个 表示： q q q 和 − q -q −q（转 720 ° 720° 720° 才回来）。插值前必须把符号对齐，否则会绕远路。
- 旋转： R ( q ) R(q) R(q) 由 Rodrigues 公式得到（见 1.4）。
- 插值 SLERP（轨迹规划用它）：
q ( t ) = sin ( ( 1 − t ) θ ) q 0 + sin ( t θ ) q 1 sin θ , θ = cos − 1 ( q 0 ⋅ q 1 ) q(t) = \frac{\sin((1-t)\theta)\,q_0 + \sin(t\theta)\,q_1}{\sin\theta},\quad \theta=\cos^{-1}(q_0\cdot q_1) q(t)=sinθsin((1−t)θ)q0+sin(tθ)q1,θ=cos−1(q0⋅q1)
真实数据代入（六轴） ：从单位姿态 q 0 = ( 1 , 0 , 0 , 0 ) q_0=(1,0,0,0) q0=(1,0,0,0) 插到绕 Z Z Z 转 90 ° 90° 90° 的姿态 q 1 = ( cos 45 ° , 0 , 0 , sin 45 ° ) q_1=(\cos45°,0,0,\sin45°) q1=(cos45°,0,0,sin45°)（代码示例5）：
| 插值点 t t t | 四元数 ( w , x , y , z ) (w,x,y,z) (w,x,y,z) | 等效旋转角 |
|---|---|---|
| 0.0 | ( 1.000 , 0 , 0 , 0 ) (1.000,\ 0,\ 0,\ 0) (1.000, 0, 0, 0) | 0 ° 0° 0° |
| 0.5 | ( 0.924 , 0 , 0 , 0.383 ) (0.924,\ 0,\ 0,\ 0.383) (0.924, 0, 0, 0.383) | 45 ° 45° 45° |
| 1.0 | ( 0.707 , 0 , 0 , 0.707 ) (0.707,\ 0,\ 0,\ 0.707) (0.707, 0, 0, 0.707) | 90 ° 90° 90° |
角度线性变化------末端走最短路径（"绕一个固定轴转过去"）。用欧拉角插值会先翻滚再摆正。
在真实控制系统里 ：MoveIt 的 orientation 插值、TF 的 rotation、相机标定 rvec/tvec 换算，全走四元数。ROS 里 geometry_msgs/Quaternion 顺序就是 (w,x,y,z)。
工程坑 ：四元数符号不统一 → 插值绕远路。判断法则：
dot(q0,q1) < 0时先取q1 = -q1。
1.4 轴角 / Rodrigues ------ "控制律里的主角"
绕单位轴 ω ^ \hat{\omega} ω^ 转 θ \theta θ：
R = I + sin θ ω × + ( 1 − cos θ ) ω × 2 , ω × = 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 R = I + \sin\theta\,\\omega\times + (1-\cos\theta)\,\\omega\times^2,\qquad \\omega_\times = \begin{bmatrix}0&-\omega_z&\omega_y\\\omega_z&0&-\omega_x\\-\omega_y&\omega_x&0\end{bmatrix} R=I+sinθω×+(1−cosθ)ω×2,ω×= 0ωz−ωy−ωz0ωxωy−ωx0
真实数据代入（七轴） ：绕 Y Y Y 轴转 90 ° 90° 90°（代码示例3）：
Rodrigues 算得 R = 0 0 1 0 1 0 − 1 0 0 , 与标准 R y ( 90 ° ) 误差 0 \text{Rodrigues 算得 } R = \begin{bmatrix}0&0&1\\0&1&0\\-1&0&0\end{bmatrix},\quad \text{与标准 } R_y(90°) \text{ 误差 } 0 Rodrigues 算得 R= 00−1010100 ,与标准 Ry(90°) 误差 0
这是伺服控制的核心 ：姿态误差 = 两个姿态矩阵的差，用轴角 表示后直接就是一个 3 维角速度误差，能喂进比例控制律。示例4：任意合成姿态提取出等效轴 ω = 0.032 , 0.470 , 0.882 \omega=0.032,\\ 0.470,\\ 0.882 ω=0.032, 0.470, 0.882、等效角 48.4 ° 48.4° 48.4°，重建回 R R R 误差 0 0 0。姿态误差天然落在角速度空间------这就是"姿态环能像位置环一样写"的原因。
在真实控制系统里 ：MoveIt 实时伺服的 computePoseError 就是这么干的：误差 = AxisAngle(R_des · R_curᵀ)。Eigen 的 AngleAxis、tf2::Matrix3x3 都有现成实现，但要理解原理。
工程坑 ：轴角提取在 θ ≈ 0 \theta \approx 0 θ≈0 时数值退化（除以 sin θ \sin\theta sinθ），要加阈值保护； θ ≈ π \theta \approx \pi θ≈π 时轴方向不唯一。四元数→轴角比矩阵→轴角数值更稳，建议一律走四元数。
2. 齐次变换与 SE(3)
T = R p 0 1 ∈ S E ( 3 ) T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \in SE(3) T=R0p1∈SE(3)
通俗理解 ：把"转"和"挪"装进一个 4 × 4 4\times4 4×4 矩阵，一次矩阵乘法同时完成旋转+平移------这就是 TF 树里的每一根"边"。
- 变换链： T w o r l d e e = T w o r l d b a s e T b a s e e e T_{world}^{ee} = T_{world}^{base}\,T_{base}^{ee} Tworldee=TworldbaseTbaseee（顺序不可交换：先转再挪 ≠ 先挪再转）
- 逆变换： T − 1 = R T − R T p 0 1 T^{-1} = \begin{bmatrix} R^T & -R^Tp \\ 0 & 1 \end{bmatrix} T−1=RT0−RTp1（一次转置 + 一次乘法，比通用求逆快且数值稳）
真实数据代入（六轴） ：base→link1 平移 ( 0.5 , 0 , 0.4 ) (0.5,0,0.4) (0.5,0,0.4) 且绕 Z Z Z 转 30 ° 30° 30°；link1→tool 沿 Z Z Z 伸出 150 m m 150mm 150mm（代码示例6）：
T b a s e t o o l = T b a s e l i n k 1 T l i n k 1 t o o l = 0.866 − 0.5 0 0.5 0.5 0.866 0 0 0 0 1 0.55 0 0 0 1 T_{base}^{tool} = T_{base}^{link1}\,T_{link1}^{tool} = \begin{bmatrix}0.866&-0.5&0&0.5\\0.5&0.866&0&0\\0&0&1&0.55\\0&0&0&1\end{bmatrix} Tbasetool=Tbaselink1Tlink1tool= 0.8660.500−0.50.8660000100.500.551
工具尖在 tool 系坐标 ( 0 , 0 , 0.15 ) (0,0,0.15) (0,0,0.15) → 在基座系 ( 0.5 , 0 , 0.7 ) m (0.5, 0, 0.7)\ \text{m} (0.5,0,0.7) m。验证 T − 1 T = I T^{-1}T = I T−1T=I 误差 0 0 0。
在真实控制系统里 ：URDF/SRDF 里的 <origin>、TF 树的 base_link → ... → tool_link、手眼标定结果，全是这种 4 × 4 4\times4 4×4 矩阵。每多一个 link 就多乘一个 T T T ；标定 TCP 就是在找 flange→tool 这一个 T T T。
工程坑
坑 现象 解法 变换顺序反了 末端位置对但姿态全错 链式乘法只写 T_ab = T_ac @ T_cb用 np.linalg.inv求 S E ( 3 ) SE(3) SE(3) 逆数值差、慢 用 T − 1 = R T − R T p 0 1 T^{-1}=\begin{bmatrix}R^T&-R^Tp\\0&1\end{bmatrix} T−1=RT0−RTp1 TF 树坐标帧名打错 末端"漂"到诡异位置 rqt_tf_tree检查父子关系手眼标定的 T T T 没验证 视觉抓取偏 标定后用"示教点反算"验证误差
3. 李代数：角速度与 twist
3.1 为什么角速度是 3 维向量
R ˙ = ω × R \dot{R} = \\omega_\times R R˙=ω×R
通俗理解 ：旋转矩阵的时间导数，恒等于"叉积矩阵 × \times × 它自己"。所以"转速"天然就是 3 个数（绕哪个轴、多快），不用存 9 个数的矩阵。这正是姿态误差用轴角表达的深层原因------误差出现在速度空间，控制律顺手。
3.2 指数映射（矩阵的"e 的 x 次方"）
- 旋转： R = exp ( ω × θ ) R = \exp(\\omega_\times \theta) R=exp(ω×θ) ------ 和 Rodrigues 完全等价
- 对数映射（轴角提取）： θ = cos − 1 ( t r ( R ) − 1 2 ) \theta = \cos^{-1}\!\big(\frac{\mathrm{tr}(R)-1}{2}\big) θ=cos−1(2tr(R)−1)，轴由 R R R 的反对称部分给出
真实数据代入（六轴） ：末端在 0.5 s 0.5\ \text{s} 0.5 s 内绕 Z Z Z 转 30 ° 30° 30°，角速度 ω z = 30 ° / 0.5 s = 1.05 rad/s \omega_z = 30° / 0.5\ \text{s} = 1.05\ \text{rad/s} ωz=30°/0.5 s=1.05 rad/s。控制律里每周期算 θ = AxisAngle(R_des · R_curᵀ).angle()，乘增益直接当角速度指令下发。
3.3 空间速度 twist V = ( ω , v ) \mathcal{V} = (\omega, v) V=(ω,v)
- ω \omega ω：角速度（rad/s）， v v v：线速度（m/s）
- MoveIt 实时伺服的输入
TwistStamped就是一个 twist ：linear= v v v，angular= ω \omega ω - 换参考系用伴随变换 A d T \mathrm{Ad}_T AdT：
V b = A d T b e V e , v b = R v e + p × ( R ω e ) , ω b = R ω e \mathcal{V}b = \mathrm{Ad}{T_{be}}\,\mathcal{V}_e,\qquad v_b = R\,v_e + p \times (R\,\omega_e),\qquad \omega_b = R\,\omega_e Vb=AdTbeVe,vb=Rve+p×(Rωe),ωb=Rωe
真实数据代入（六轴） （代码示例7）：末端系里绕自身 z z z 转 0.3 rad/s 0.3\ \text{rad/s} 0.3 rad/s、沿自身 x x x 移 0.1 m/s 0.1\ \text{m/s} 0.1 m/s；末端在基座系位姿 T = ( R z ( 30 ° ) , p = ( 0.5 , 0.2 , 0.3 ) ) T=(R_z(30°),\ p=(0.5,0.2,0.3)) T=(Rz(30°), p=(0.5,0.2,0.3))：
ω b = ( 0 , 0 , 0.3 ) , v b = ( 0.147 , − 0.1 , 0 ) m/s \omega_b = (0,\ 0,\ 0.3),\qquad v_b = (0.147,\ -0.1,\ 0)\ \text{m/s} ωb=(0, 0, 0.3),vb=(0.147, −0.1, 0) m/s
注意线速度多出一项 p × ( R ω e ) p\times(R\omega_e) p×(Rωe) ------ 臂展放大效应：末端在转，基座看它线速度更大。这就是"手眼标定里速度/角速度必须统一到同一参考系"的数学原因。
在真实控制系统里：遥操/伺服命令、力控里的目标速度、双臂相对速度全用 twist 表达；两臂协同（09 篇）就是两套 twist 的差。
工程坑 ：twist 是相对哪个系写的必须说清（
frame_id填错 = 速度方向全错）。servo 输入默认按tool系还是base系解算，不同配置差别很大。
4. 雅可比与速度级运动学
v = J ( q ) q ˙ v = J(q)\,\dot{q} v=J(q)q˙
- J ∈ R 6 × n J \in \mathbb{R}^{6\times n} J∈R6×n， n n n = 关节数。七轴 n = 7 n=7 n=7，雅可比是瘦高矩阵 → 冗余。
- 几何雅可比：逐列由关节轴 twist 叉乘得到；数值雅可比（中心差分）用于验证。
4.1 真实数据代入（七轴）
取七轴臂一组关节角（代码示例8），算空间雅可比 J ( 6 × 7 ) J(6\times7) J(6×7)，奇异值：
σ = 2.101 , 1.697 , 1.258 , 0.525 , 0.309 , 0.151 \sigma = 2.101,\\ 1.697,\\ 1.258,\\ 0.525,\\ 0.309,\\ 0.151 σ=2.101, 1.697, 1.258, 0.525, 0.309, 0.151
最小的 σ m i n = 0.151 \sigma_{min}=0.151 σmin=0.151 还不算奇异，但已经说明某些方向末端几乎动不了 。末端目标速度 v = ( 0.2 , − 0.1 , 0.05 , 0 , 0 , 0.3 ) v=(0.2,-0.1,0.05,0,0,0.3) v=(0.2,−0.1,0.05,0,0,0.3)，最小范数关节速度：
q ˙ = J † v = ( 0.233 , 0.107 , 0.129 , − 0.504 , − 0.428 , 0.394 , 0.402 ) \dot{q} = J^\dagger v = (0.233,\ 0.107,\ 0.129,\ -0.504,\ -0.428,\ 0.394,\ 0.402) q˙=J†v=(0.233, 0.107, 0.129, −0.504, −0.428, 0.394, 0.402)
验证 J q ˙ = v J\dot{q} = v Jq˙=v，误差 0 0 0。工程含义：MoveIt / 伺服里"末端速度 → 关节速度"全靠这一行。
4.2 伪逆（冗余求解的核心）
J † = J T ( J J T ) − 1 ( 满行秩 ) J^\dagger = J^T(JJ^T)^{-1} \quad(\text{满行秩}) J†=JT(JJT)−1(满行秩)
用 SVD 更稳： J = U Σ V T J = U\Sigma V^T J=UΣVT， J † = V Σ † U T J^\dagger = V\Sigma^\dagger U^T J†=VΣ†UT。伪逆给出最小范数解（所有能实现该末端速度的解里，关节速度最省的那个）。
4.3 阻尼最小二乘（奇异鲁棒）
J ∗ = J T ( J J T + λ 2 I ) − 1 J^* = J^T(JJ^T + \lambda^2 I)^{-1} J∗=JT(JJT+λ2I)−1
真实数据代入（七轴） （代码示例9）：把最小奇异值压到 σ m i n = 0.001 \sigma_{min}=0.001 σmin=0.001（病态）：
| 方法 | 关节速度范数 ∣ q ˙ ∣ |\dot{q}| ∣q˙∣ | 说明 |
|---|---|---|
| 普通伪逆 | 21.2 21.2 21.2 | 解爆炸，电机指令打到天文数字 |
| 阻尼伪逆（ λ = 0.01 \lambda=0.01 λ=0.01） | 0.22 0.22 0.22 | 被压住，代价是末端跟踪有少量误差 |
奇异时 σ m i n → 0 \sigma_{min} \to 0 σmin→0， λ \lambda λ 兜底------这就是为什么伺服/IK 求解器里全是"阻尼"二字的数学原因。
4.4 冗余的一般解（七轴的"聪明"所在）
q ˙ = J † v + ( I − J † J ) q ˙ 0 \dot{q} = J^\dagger v + (I - J^\dagger J)\,\dot{q}_0 q˙=J†v+(I−J†J)q˙0
- 第一项：最小范数解（主任务 = 末端速度）
- 第二项：零空间投影 ， q ˙ 0 \dot{q}_0 q˙0 是次要任务梯度，不影响主任务
真实数据代入（七轴） （代码示例10）：末端只要 v = ( 0.15 , 0 , 0 , 0 , 0 , 0 ) v=(0.15,0,0,0,0,0) v=(0.15,0,0,0,0,0)； q ˙ 0 = 1 \dot{q}_0=\mathbf{1} q˙0=1 表示"想让每个关节都动一下"：
最小范数解： J q ˙ = ( 0.15 , 0 , 0 , 0 , 0 , 0 ) \text{最小范数解：}\ J\dot{q} = (0.15,0,0,0,0,0) 最小范数解： Jq˙=(0.15,0,0,0,0,0)
叠加零空间后： J q ˙ = ( 0.15 , 0 , 0 , 0 , 0 , 0 ) 完全不变 ✓ \text{叠加零空间后：}\ J\dot{q} = (0.15,0,0,0,0,0) \quad \text{完全不变 ✓} 叠加零空间后： Jq˙=(0.15,0,0,0,0,0)完全不变 ✓
但关节速度从 ( − 0.032 , − 0.023 , − 0.263 , 0.129 , 0.822 , − 0.098 , − 0.537 ) (-0.032,-0.023,-0.263,0.129,0.822,-0.098,-0.537) (−0.032,−0.023,−0.263,0.129,0.822,−0.098,−0.537) 变成了 ( 0.010 , − 0.028 , − 0.313 , 0.157 , 0.814 , − 0.118 , − 0.517 ) (0.010,-0.028,-0.313,0.157,0.814,-0.118,-0.517) (0.010,−0.028,−0.313,0.157,0.814,−0.118,−0.517) ------ 末端速度分毫未动，关节却重新分配了 。七轴臂"肘部自动绕开障碍、躲关节限位、避奇异"全是这一行 + 合适的 q ˙ 0 \dot{q}_0 q˙0（02 篇展开）。
工程坑
坑 现象 解法 雅可比参考系不一致 末端速度对，姿态方向错 统一用 base 系或 tool 系，写清楚 奇异附近硬用伪逆 关节速度爆表 → 伺服报警 用阻尼伪逆 + 奇异值监控 零空间梯度太强 主任务被拖偏 零空间项乘一个小的系数（如 0.1） 数值雅可比步长过大 差分误差大 步长 10 − 6 ∼ 10 − 7 10^{-6}\sim10^{-7} 10−6∼10−7，只用来验证解析解
5. 概率与滤波基础
5.1 高斯分布与协方差
传感器融合/状态估计的最小二乘视角：最小化 ∥ y − H x ∥ W 2 \|y - Hx\|^2_W ∥y−Hx∥W2， W W W 越大越信谁。协方差矩阵就是"这个传感器的话有多可信"。
5.2 卡尔曼滤波（KF/EKF）
x ^ k ∣ k − 1 = A x ^ k − 1 + B u k P k ∣ k − 1 = A P k − 1 A T + Q K k = P k ∣ k − 1 H T ( H P k ∣ k − 1 H T + R ) − 1 x ^ k = x ^ k ∣ k − 1 + K k ( z k − H x ^ k ∣ k − 1 ) P k = ( I − K k H ) P k ∣ k − 1 \begin{aligned} \hat{x}{k|k-1} &= A\hat{x}{k-1} + Bu_k \\ P_{k|k-1} &= A P_{k-1} A^T + Q \\ K_k &= P_{k|k-1}H^T(HP_{k|k-1}H^T + R)^{-1} \\ \hat{x}k &= \hat{x}{k|k-1} + K_k(z_k - H\hat{x}{k|k-1}) \\ P_k &= (I - K_kH)P{k|k-1} \end{aligned} x^k∣k−1Pk∣k−1Kkx^kPk=Ax^k−1+Buk=APk−1AT+Q=Pk∣k−1HT(HPk∣k−1HT+R)−1=x^k∣k−1+Kk(zk−Hx^k∣k−1)=(I−KkH)Pk∣k−1
真实数据代入（六轴） （代码示例11）：250 Hz 控制周期、编码器位置噪声 ± 2 mm \pm 2\ \text{mm} ±2 mm、用"位置 + 常速度"两状态 KF 估计关节速度：
| 方法 | 速度估计 RMSE (rad/s) |
|---|---|
| 直接中心差分 | 0.359 0.359 0.359 |
| 卡尔曼滤波 | 0.059 0.059 0.059（改善 84%） |
为什么必须滤波 ：位置差分会把编码器噪声放大（除以很小的时间步长 d t dt dt）。位置控制里速度反馈直接用差分会抖------这就是"速度反馈要用 KF/观测器提纯"的量化证据。
在真实控制系统里 ：关节速度估计、力传感器滤波、末端位姿估计（融合编码器 + 相机）、双臂相对位姿估计都用 KF/EKF。robot_localization、control_toolbox 都是现成轮子，但 Q/R 要自己调。
工程坑
坑 现象 解法 Q Q Q 太大 R R R 太小 滤波反应过激、跟随噪声 从"模型可信度"出发设 Q Q Q，实测调 R R R 太大 Q Q Q 太小 滤波滞后严重 看预报误差 ( z k − H x ^ ) (z_k - H\hat x) (zk−Hx^) 的实测方差 EKF 线性化点不对 发散 检查雅可比、采样时间 d t dt dt 忘了把单位换算一致 数值诡异 位置 rad、速度 rad/s，别混
6. 数值方法速查
| 问题 | 方法 | 备注 |
|---|---|---|
| 伪逆 | SVD | 奇异值就是"奇异程度"的直接读数 |
| 数值 IK | LM（阻尼最小二乘） | 阻尼 + 自适应步长，本质是 §4.3 的迭代版 |
| 约束优化 | SQP / 投影梯度 | 双臂内力、零空间 |
| 矩阵求逆 | LU / Cholesky（对称正定） | 阻抗/MPC 的 KKT 系统 |
| 微分 | 中心差分（仅验证） | 控制里一律用解析雅可比 |
| 姿态插值 | 四元数 SLERP | 别用欧拉角 |
7. 代码示例（code/01_examples.py）
11 个示例，建议逐个跑一遍并改参数观察：
| 示例 | 内容 | 关键输出 |
|---|---|---|
| ex1 | 旋转矩阵 R z ( 30 ° ) R_z(30°) Rz(30°) 正交性 | R T R − I = 0 , det = + 1 R^TR-I=0,\ \det=+1 RTR−I=0, det=+1 |
| ex2 | ZYX 欧拉角 + 万向锁 | pitch=90° 时 roll/yaw 等价 |
| ex3 | 轴角 Rodrigues 绕 Y Y Y 转 90° | 与 R y ( 90 ° ) R_y(90°) Ry(90°) 误差 0 |
| ex4 | 旋转矩阵 → 轴角提取 | 轴 0.032 , 0.470 , 0.882 0.032,0.470,0.882 0.032,0.470,0.882、角 48.4 ° 48.4° 48.4° |
| ex5 | 四元数 SLERP 插值 | t = 0.5 → 45 ° t=0.5\to45° t=0.5→45°，线性 |
| ex6 | SE(3) 变换链 + 逆 | 工具尖 ( 0.5 , 0 , 0.7 ) (0.5,0,0.7) (0.5,0,0.7) m |
| ex7 | twist 伴随变换 | 臂展放大效应 v b = ( 0.147 , − 0.1 , 0 ) v_b=(0.147,-0.1,0) vb=(0.147,−0.1,0) |
| ex8 | 七轴雅可比 + 伪逆 | J q ˙ = v J\dot q=v Jq˙=v 误差 0 |
| ex9 | 阻尼伪逆抗奇异 | ∣ q ˙ ∣ : 21.2 → 0.22 |\dot q|: 21.2 \to 0.22 ∣q˙∣:21.2→0.22 |
| ex10 | 冗余零空间 | 末端速度不变、关节重分配 |
| ex11 | KF 速度估计 | RMSE 改善 84% |
核心函数 hat() / rot_axis_angle() / axis_angle_from_rot() / damped_pinv() 建议和 Eigen 对照写一遍，7 轴里到处都是它们。
8. 对应到你的技术栈
| 概念 | MoveIt2 / ros2_control 位置 |
|---|---|
| SE(3) 变换 | TF2：base_link → tool_link（/tf 话题） |
| twist | geometry_msgs/TwistStamped（servo 输入） |
| 雅可比 | moveit_core 的 RobotState::getJacobian()；KDL Jacobian |
| 阻尼伪逆 | MoveIt 数值 IK 内部（ik_solver_base）；servo 奇异处理 |
| 姿态误差（轴角） | MoveIt 实时伺服的 computePoseError；Eigen AngleAxis |
| 卡尔曼滤波 | 关节速度估计、力传感器滤波（robot_localization） |
练习
- 用
rot_axis_angle拼出 ZYX 欧拉角旋转，验证等价旋转矩阵 - 验证 q q q 与 − q -q −q 表示同一旋转；SLERP 前做
dot<0符号校正 - 对七轴 J ( 6 × 7 ) J(6\times7) J(6×7) 用阻尼伪逆求最小范数解，观察奇异值变化时解的行为（对照示例9）
- 手写 EKF，对带噪声的位置序列估计速度，对比直接差分（对照示例11）
- 把示例6 的变换链加一级
tool→camera，算出手眼标定里的T_base_camera
上一篇：00_机械臂基础概念.md
下一篇：02_七轴运动学.md