运动学：关节怎样决定末端位置

构型、自由度与工作空间

机器人构型 \(\mathbf q\) 是描述机构姿态所需的最小变量集合。转动关节用角度，移动关节用位移。六个独立转动关节常能在三维空间控制末端的三维位置和三维方向，因此工业机械臂常见六轴；增加第七轴产生冗余，让机械臂在末端不动时仍能改变肘部姿态，便于避障和远离奇异点。

自由度数量不能单独说明能力。两轮差速底盘的平面位姿有 \(x,y,\theta\) 三个变量，但它不能瞬间横移，属于非完整约束；某些机械臂虽有六轴，在特定构型仍会失去一个运动方向。工作空间描述末端能到达的区域，可达工作空间只要求某个方向能到达，灵巧工作空间要求每个所需方向都能到达。

正运动学：沿着机构把变换乘起来

串联机械臂可以看成从底座开始的变换链：每个关节贡献一个随 \(q_i\) 变化的变换，每根连杆贡献一个固定变换。末端位姿为

\[{}^{0}\mathbf T_n(\mathbf q)={}^{0}\mathbf T_1(q_1){}^{1}\mathbf T_2(q_2)\cdots{}^{n-1}\mathbf T_n(q_n).\]

Denavit–Hartenberg（DH）参数用四个量描述相邻关节坐标系的关系，适合规则串联机构和教学。乘积指数公式则把每个关节写成李群指数 \(e^{[\mathcal S_i]q_i}\)，便于统一处理关节螺旋轴：

\[\mathbf T(\mathbf q)=e^{[\mathcal S_1]q_1}e^{[\mathcal S_2]q_2}\cdots e^{[\mathcal S_n]q_n}\mathbf M.\]

\(\mathcal S_i\) 是第 \(i\) 个关节在空间坐标系下的螺旋轴，\([\mathcal S_i]\) 是对应矩阵形式，\(\mathbf M\) 是所有关节为零时的末端位姿。两种方法描述的是同一几何链，选择取决于团队工具和模型来源。

对最简单的平面二连杆，末端位置可以直接看见几何含义：

\[x=l_1\cos q_1+l_2\cos(q_1+q_2),\]

\[y=l_1\sin q_1+l_2\sin(q_1+q_2).\]

第一根连杆贡献长度为 \(l_1\)、方向为 \(q_1\) 的向量，第二根贡献长度为 \(l_2\)、方向为 \(q_1+q_2\) 的向量；末端位置就是两者相加。这个例子把“沿机构乘变换”还原成直观的向量叠加。

逆运动学：可能多解、无解或不稳定

逆运动学求解

图 1　正运动学通常唯一，逆运动学天然可能多解。

\[\mathbf T(\mathbf q)=\mathbf T_d,\]

其中 \(\mathbf T_d\) 是期望末端位姿。平面二连杆到同一点通常有“肘上”和“肘下”两组解；目标在最大臂展之外时无解；冗余机械臂可能有无穷多解；靠近奇异点时，小小的末端变化会要求很大的关节变化。

对上述二连杆，余弦定理给出

\[\cos q_2=\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2}.\]

只要右侧位于 \([-1,1]\)，\(q_2\) 通常有正负两支，对应肘部两种姿态；超出该区间意味着目标不可达。再由目标方向与三角关系求 \(q_1\)。真实六轴机械臂的解析解更复杂，但“检查可达性—枚举分支—按约束选解”的逻辑相同。

解析法利用机构几何直接给出有限组解，速度快且可枚举，但只适用于结构已知、可推导的机构。数值法从初值出发迭代。最基本的雅可比伪逆更新为

\[\Delta\mathbf q=\mathbf J^{\dagger}(\mathbf q)\,\mathbf e,\]

\(\mathbf e\) 是当前末端位姿与目标的局部误差，\(\mathbf J^{\dagger}\) 是伪逆。靠近奇异点时常用阻尼最小二乘：

\[\Delta\mathbf q=\mathbf J^T(\mathbf J\mathbf J^T+\lambda^2\mathbf I)^{-1}\mathbf e.\]

\(\lambda>0\) 是阻尼。阻尼越大，关节变化越温和，但目标误差下降得更慢。逆运动学还要满足关节限位、自碰撞、外部碰撞和姿态偏好，常写成约束优化：

\[\min_{\mathbf q}\; \|\mathbf e(\mathbf q)\|_{\mathbf W}^2+\alpha\|\mathbf q-\mathbf q_0\|^2, \quad \text{s.t. }\mathbf q_{\min}\le \mathbf q\le\mathbf q_{\max}.\]

第一项追踪末端目标，第二项让解靠近舒适姿态 \(\mathbf q_0\)，\(\alpha\) 决定二者权衡。工程上还会加入碰撞距离和速度限制。

微分运动学与零空间

雅可比伪逆可以把期望末端速度映射成关节速度。对冗余机器人，通解可写为

图 2　阻尼用少量跟踪误差换取有限的关节速度。

\[\dot{\mathbf q}=\mathbf J^{\dagger}\mathbf V_d+ (\mathbf I-\mathbf J^{\dagger}\mathbf J)\mathbf z.\]

第一项完成主任务，第二项位于雅可比零空间，不改变瞬时末端速度。\(\mathbf z\) 可以用来远离关节限位、增大可操作度或避开障碍。不过“瞬时不影响”不保证长时间绝对不影响，离散化、模型误差和任务优先级切换都可能泄漏到主任务。

可操作度、标定误差与解的连续性

雅可比的奇异值能描述机器人在不同方向上的瞬时运动能力。某个奇异值很小，表示该方向的末端速度需要很大的关节速度；奇异值之比很大，说明映射病态，测量噪声和数值误差会被放大。常见可操作度指标为

\[w(\mathbf q)=\sqrt{\det\bigl(\mathbf J(\mathbf q)\mathbf J^T(\mathbf q)\bigr)}.\]

它接近零时提示奇异，但单个标量会掩盖究竟丢失哪个方向，也受单位和雅可比尺度影响。工程中应同时查看最小奇异值、条件数和任务方向；若任务只要求沿桌面移动，另一个无关方向能力较弱可能仍可接受。

运动学模型还依赖几何参数。连杆长度、关节零偏、底座位置、工具中心点和相机外参任何一项有误，正运动学都会给出系统偏差。标定通常采集多姿态下的外部测量，最小化模型预测与真实观测的差异。采样姿态要覆盖工作空间并激励不同参数；若机械臂只在狭小区域摆动，多个参数可能互相补偿，训练点残差很小，换到另一姿态便明显偏离。

标定验收需要使用未参与拟合的姿态，分别报告位置、方向和重复性。关节回差、结构柔性和负载下挠随运动方向或载荷变化，固定几何参数无法完全吸收；这类误差需要方向相关补偿、刚度模型或视觉闭环。把所有残差继续塞进 DH 参数，会得到缺乏物理意义且无法外推的模型。

逆运动学的多解还带来连续性问题。逐个轨迹点独立选择“距离最近”的解，在两支解代价相近时可能突然换支，引发腕部翻转。稳定做法是以上一时刻解为初值，对关节变化、奇异性和限位余量加代价，并对周期关节做角度展开。若确需换支，应规划一段显式过渡并重新做碰撞检查。

工程例子：六轴机械臂把相机探入货架

任务要求相机保持朝向货物，同时沿直线进入狭窄货架。末端位姿满足不代表肘部安全；某个解析 IK 解可能让腕部翻转，另一个解可能碰货架。工程实现应先筛掉越限与碰撞解，再按与当前关节状态的距离、奇异性和线缆扭转评分。执行中用连续初值保持解分支，避免相邻轨迹点突然从“肘上”跳到“肘下”。

失败边界包括：目标恰在工作空间边界，数值解会抖动；位姿六个方向都设为硬约束，狭窄环境可能无解；工具长度写错会造成系统性位置偏差；只检查离散轨迹点，连点之间仍可能碰撞。更稳妥的做法是允许非关键方向有容差，并对整段连续运动做碰撞验证。