2025-08-19
Kinematics：运动学，关心位移、线/角速度、线/角加速度等信息. 不关心其背后的力学. 即只关心位移以及其各阶导数.
Dynamics：力学，关心如何通过力的作用来达成运动. 通过 \(F=ma\) 和运动学连接.
我们更关心 Kinematics. 实际中，通过 Control 来连接 Dynamics 和 Kinematics，而这个大多数情况已经很成熟了.
Rigid Motion 只有平移和旋转. 注意镜像不是 Rigid Motion. Rigid Motion 需要保证 distance-perserving（保距） 和 chirality-perserving（保手性）.
从一个观察者 \(s\) 的角度，定义其 frame（参考系）为 \(\mathscr F_s\). 在写算式的时候，我们将参考系的 \(s\) 放在上角标，如 \(o_b^s=o_{s}^s+\bold t_{s\to b}^s\).
对于一个 rigid \(b\)，我们可以 bind a frame \(\mathscr F_b\)to this object，然后这个 frame 就会跟着这个 object 动（无论是平移还是选转）. 也就是说 object 上的点 \(p\)，无论 object 怎么动，其在 \(\mathscr F_b\) 下的坐标 \(p^b\) 都不会变.
rigid 的 Pose：如何将 \(\mathscr F_s\) 经过平移和旋转变成 \(\mathscr F_b\). Rotation 和 Translation 的 DoF（自由度）都是 \(3\). 于是加起来，Pose 的 DoF 是 \(6\).
如何描述 Transformation？我们考虑先平移，再旋转. 平移的公式是 \(o_b^s=o_{s}^s+\bold t_{s\to b}^s\)；旋转的公式是 \([\bold x,\bold y,\bold z]_b^s=R_{s\to b}^s [\bold x,\bold y,\bold z]_{s}^{s}\). 于是在 \(\mathscr F_s\) 中，这个 Transformation 等同于 pose.
对于 object 上的点 \(p\)，我们就有 \(p^s=R^s_{s\to b}p^b+t^{s}_{s\to b}\).
用矩阵描述这个表示，这个在 cvdl 已有提及.
从 \(p^b\) 变换到 \(p^s\) 的变换可以用二元组 \((R^s_{s\to b},t^s_{s\to b})\) 表示. 但是显然这个变换关于 \(p\) 非线性. 这主要是因为 translation 导致的. 于是引入 Homogeneous coordinate，直接加一个维度变成 \((x,y,z,1)\)，然后得到 Homogeneous coordinate 下的 transformation matrix：\(\left(\begin{matrix}R^s_{s\to b} & t_{s\to b} \\ 0 & 1\end{matrix}\right)\).
使用这个就可以直接写 \(p^{1}=T^1_{1\to 2}p^2\). 然后复合也是类似的，\(T^3_{3\to 1}=T^3_{3\to 2}T^2_{2\to 1}\)；\(T^1_{1\to 2}=(T^{2}_{2\to 1})^{-1}\). 稍微想一下就可以直到这个的用处很大，毕竟机械臂一节一节的就可以用变换传递过去，形成一个 Kinematic Chain.
Links：rigid body，每个段
Joints：links 之间的连接器（关节），决定了相邻 links motion 的 DoF.
Base Link / Root Link：0th link，看作固定的 \(s\)，以此建立 \(\mathscr F_s\).
End-effector link：last link，去 do the job 的部分. 比如说一个 gripper. 其 frame 称为 \(\mathscr F_{e}\).
Revolute Joint：1D 旋转，即只在一个平面旋转一个角度.
Prismatic Joint：1D 平移，即只在一个方向移动一个距离.
一个想到的事：需要注意 frame 是 bind 在 link 上的！
Joint Space：记录着每个 Joint 的更改角度的.
Cartesian Space：记录每个 Link 的 frame.
这两个 space 显然是可以互相转换的. 称从 Joint Space 到 cartesian space 的操作为 Forward Kinematics
相反，Inverse Kinematics，就是得知要操作的位置和初始位置，然后设计一个这样的 Joint Space.
根据解方程那一套理论，如果你的系统的 DoF 是 \(6\) 的话，只要你想要的结果是 within reachability 的，那么就可以有唯一的方案. 而有些系统的 DoF 是 \(7\) 的话，那么就有无穷的方案，就可以避开障碍.
人的手臂的 DoF 就是 \(7\). 肩关节有 \(3\)，手腕是 \(2\)，肘关节是 \(2\).
解 IK 很困难. 有的特定的 6-DoF 有特定的解，而其他需要通过一些 Optimization 的方法解决.
SO：特殊正交群. 即旋转群.
SE：特殊欧几里得群. 即 \(\left(\begin{matrix}R & t \\ 0 & 1\end{matrix}\right)\) 群.
机器人的计算位置有云-边-端三个.
具身机器人的控制频率在 250Hz ~ 1000Hz 左右，大概就是 4ms 要完成计算，于是有些运算就一定要放在端上. 一些比较复杂的步骤可以在云上计算. 边则是机器人肚子里的一个模型. 在端上的芯片（比如 Orion X）的算力非常小，甚至不如 3090.
用旋转矩阵来表示旋转会有若干的问题：首先有 3 个自由度但有 \(9\) 个数，徒增运算负担；然后运算精度误差会使得失去正交性，要时不时进行 schmidt 正交化. 当然 schmidt 正交化确实也会有时偏颇，因为和矩阵向量顺序有关，这没什么道理. 一个方法是，SVD 分解后，中间的对角矩阵应该是一个每个东西都接近 \(1\) 的东西，所以直接把其改成 \(1\) 然后再乘回去.
所以我们选择四元数！更多见高代笔记. 四元数做 normalization 也很简单直接除以 magnitude 就行了. 而且计算起来计算量也少.
四元数之间距离：即球面距离，等于 \(2\arccos(|p\cdot q|)\). 这个和所代表旋转的转角成正比.
如何对旋转做插值？首先用四元数，然后我们其实就是对一个弧做弧线上的均匀插值. 这用到 slerp 算法（spherical linear interpolation）：令 \(\phi=\arccos(q_0\cdot q_1)\)，然后用初中几何知识可以推得 \(q(t)=\frac{q_0\sin(1-t)\phi+q_1\sin t\phi}{\sin\phi}\).
在 SO(3) 中的均匀采样：首先四元数的理论，等同于 S(3) 中的均匀采样. 取 \(\mathscr N(0,I_{4})\) 然后再 normalize. 均匀采样很重要！不均匀采样会导致 training set 和 test 的分布不同，会导致一个天然的 drop.
但其实 representation 的学问很深. 四元数虽好但是还是有可能有这种突变，以及还要综合考量各种 gradient 什么的东西.
Motion Planning
Control System
Sensor：有自身的感知信息（preprioceptive info），以及 camera 这种视觉信息.
Actuator：真正要去实施调整的地方，比如 joint motor
Controller：根据信息给出 feedback 然后给出调整.
Open loop（feed forward）control：直接根据 input 给出 output，没有 feedback. 在理想情况下是对的.
Closed loop（feed back）control：会将出现的误差 Error 反馈回来，然后继续进行调整. 我们需要减小 steady state 的 error，然后要快速停止震荡.
Overshoot：操作过头了，比如 10cm 结果变成了 11cm.
Settling time：从第一次的原始误差，变化到最后的可接受的误差所需要的时间.
令 \(\theta(t)\) 为 \(t\) 时刻位置，\(\theta_d(t)\) 为 \(t\) 时刻目标位置，\(\theta_e(t)\) 为 \(t\) 时刻误差，\(e_{ss}\) 为 \(t\to \infty\) 时的误差.
Proprotional Control：选择一个常数 \(K_p\)，然后直接用 \(K_p\) 作为调整. 也就是说，令 \(\theta'(t)=K_p\theta_e(t)\). 即 \(\theta'_d(t)-\theta'_e(t)=K_p\theta_e(t)\)，而由于 \(\theta'_d(t)=0\) 所以 \(\theta'_e(t)=-K_p\theta_e(t)\). 用小学常微分方程知识知道 \(\theta_e(t)=\theta_e(0)\times e^{-tK_p}\).
P Control 在 \(\theta_d(t)\) 存在变化的时候显然就有问题了，以及如果用 P 作为二阶导（加速度）的 control 那么就不收敛了. 于是提出 Integral Control：\(I=K_i\int_0^t \theta_e(\tau)d\tau\).
结合在一起，有 PI Control：\(PI=K_p\theta_e(t)+K_i\int_0^t\theta_e(\tau)d\tau\). 我们用 \(PI\)
可以据此列出方程 \(\theta''_e+K_p\theta'_e+K_i\theta_e=\theta''_d\). 如果右侧取 \(0\) 那么就是一个标准的 \(\theta''_e+K_p\theta'_e+K_i\theta_e=0\).
可以感受一下，如果目标是在匀速运动的话，那么 PI Control 就可以逐渐收敛，而 P Control 不可以.
我们也可以再引入 Deriative Control. 同理，\(D=K_d \frac{d}{dt}\theta_e(t)\). 当 error 在变大的时候就加强 punishing，变小的时候就更 gentle. 结合起来就是 PD Control. 我们用 PD 去 control 其运动的二阶导，来减小 overshoot，那么就是 \(\theta''_e+K_d\theta'_e+K_p\theta_e=\theta''_d\).
结合在一起就形成了 PID Control.
其实再感受一下，\(k_p\) 相当于就是提供一个往目标的一个冲劲（stiffness），调太大会导致可能停不下来，而 \(k_d\) 就是可以作为一个 damping 去存在.