第二章 运动学约束：关节位置、速度、加速度极限，笛卡尔空间位姿约束，奇异性规避
各位同行，今天我们来聊聊轨迹规划里最让人头疼的部分——约束处理。说实话，我刚入行那会儿，觉得轨迹规划不就是算一条路径嘛，把点连起来就完事了。结果第一次上真机，机器人直接报警，关节限位、速度超限、加速度冲击，搞得我手忙脚乱。嗯，从那以后我才明白，没有约束的轨迹，就是一张废纸。
这一章，我重点讲三个硬骨头：关节运动学约束、笛卡尔空间位姿约束，还有奇异性规避。说白了，就是让你的机器人既能跑得快，又不会撞到自己的极限，更不会在奇异点附近发疯。
核心观点：约束不是限制，而是保护。你想想看，没有限速的汽车谁敢开？机器人也一样。
2.1 关节位置、速度、加速度极限
先聊关节位置极限。这个最简单，每个关节都有物理限位，比如J1只能转±180°，J2只能转±135°。我见过不少新手，规划轨迹时完全不检查关节范围，结果机器人走到一半，某个关节“咔”一声撞到硬限位。轻则报警停机，重则损坏减速机。
我的习惯是：在轨迹规划前，先做一次关节空间的可达性检查。说白了，就是把笛卡尔空间的路径点，用逆运动学转成关节角，然后逐个关节比对限位。如果某个点超限，要么调整路径，要么换一个逆解分支。
避坑指南：我曾经遇到过一个问题——逆运动学有多个解，有些解关节角在限位内，有些则超限。所以不能只算一组解就完事，要遍历所有可能的逆解分支，选一个最安全的。
接下来是速度极限。每个关节电机都有额定转速，比如J1最大速度2 rad/s，J2最大速度3 rad/s。轨迹规划时，我们要保证每个关节的实际速度不超过这个值。怎么做？其实很简单——计算轨迹的雅可比矩阵，把笛卡尔速度映射到关节速度，然后检查每个关节的速度是否超限。
我个人的经验是：速度约束往往不是瓶颈，真正麻烦的是加速度约束。为什么？因为加速度直接影响力矩，力矩大了电机就发热，发热了就得降速。你想想看，一个六轴机器人，六个电机同时发力，热管理是个大问题。
加速度约束的处理，我一般用S形速度曲线或者梯形速度曲线。梯形曲线简单粗暴，但加速度有突变，容易引起振动。S形曲线平滑，但计算量稍大。我的建议是：高精度场景用S形，普通搬运用梯形就够了。
// 关节速度/加速度约束检查伪代码
bool checkJointLimits(const JointState& q, const JointState& qd, const JointState& qdd) {
for (int i = 0; i < NUM_JOINTS; i++) {
// 位置极限
if (q[i] < q_min[i] || q[i] > q_max[i]) return false;
// 速度极限
if (fabs(qd[i]) > qd_max[i]) return false;
// 加速度极限
if (fabs(qdd[i]) > qdd_max[i]) return false;
}
return true;
}
2.2 笛卡尔空间位姿约束
关节空间的约束好理解，但实际应用中，我们更关心的是末端执行器的位姿。比如焊接时，焊枪必须垂直于焊缝；打磨时，打磨头必须与工件表面保持固定角度。这些就是笛卡尔空间的位姿约束。
笛卡尔空间的约束分两种：位置约束和姿态约束。位置约束好办，就是末端在x、y、z方向上的范围。姿态约束就复杂了，因为姿态表示方式有很多种——欧拉角、四元数、旋转矩阵，每种都有各自的坑。
我个人习惯用四元数来表示姿态约束。为什么？因为欧拉角有万向锁问题，旋转矩阵有9个参数冗余。四元数虽然抽象，但插值平滑，没有奇异性。嗯，这里要注意：四元数约束不能直接做线性插值，要用球面线性插值（SLERP）。
警告：千万不要在笛卡尔空间直接用欧拉角做线性插值！我踩过这个坑——有一次做喷涂轨迹，用欧拉角插值，结果在某个姿态附近，机器人突然反转了180度，差点把工件撞飞。后来换成四元数SLERP，问题才解决。
笛卡尔空间位姿约束的处理流程，我总结为三步：
- 定义约束空间：明确末端在哪些自由度上有约束（比如只约束位置，不约束姿态）
- 映射到关节空间：通过逆运动学，把笛卡尔约束转成关节约束
- 实时检查：在轨迹插补的每个周期，检查末端位姿是否在约束范围内
这里有个技巧：如果约束是时变的（比如焊接过程中焊枪角度逐渐变化），那就需要把约束也做成轨迹的一部分。说白了，就是同时规划位置和姿态，让它们协同变化。
2.3 奇异性规避
奇异性，是每个轨迹规划工程师的噩梦。什么是奇异点？简单说，就是机器人某个关节的运动会“失效”，导致末端在某些方向上无法运动。比如，六轴机器人的腕部奇异——当J4和J6的轴线重合时，J5就失去了一个自由度。
奇异性的危害有多大？我举个例子：有一次我做高速拾取任务，机器人经过一个奇异点附近，关节速度瞬间飙升到额定值的3倍，电机直接过流报警。更可怕的是，如果奇异点附近有工件，机器人可能会失控撞上去。
奇异性规避的方法，我常用的有三种：
| 方法 | 原理 | 适用场景 | 缺点 |
|---|---|---|---|
| 阻尼最小二乘法（DLS） | 在雅可比矩阵中加入阻尼项，避免奇异点附近速度过大 | 通用场景，尤其是连续轨迹 | 会引入轨迹误差 |
| 奇异点分离法 | 将奇异方向和非奇异方向分开处理 | 已知奇异方向的任务 | 实现复杂 |
| 路径重规划 | 检测到奇异点后，重新规划路径绕开 | 离线编程 | 实时性差 |
我个人最常用的是阻尼最小二乘法。它的核心思想是：当机器人接近奇异点时，我们不再追求精确的笛卡尔速度，而是允许一定的误差，换取关节速度的稳定。说白了，就是“保命要紧”。
// 阻尼最小二乘法伪代码
Eigen::MatrixXd dampedPseudoInverse(const Eigen::MatrixXd& J, double lambda) {
// J: 雅可比矩阵, lambda: 阻尼系数
Eigen::MatrixXd Jt = J.transpose();
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(J.rows(), J.rows());
return Jt * (J * Jt + lambda * lambda * I).inverse();
}
技巧：阻尼系数lambda怎么选？我一般用自适应阻尼——根据最小奇异值的大小动态调整。奇异值越小，阻尼越大。这样在远离奇异点时不影响精度，靠近奇异点时自动保护。
还有一种方法，我称之为“绕路走”。如果路径必须经过奇异点附近，那就干脆在奇异点前后各加一个过渡点，让机器人从旁边绕过去。虽然路径变长了，但安全啊。你想想看，是慢一点好，还是撞机好？
最后，我想强调一点：奇异性不是bug，是物理规律。任何串联机器人都有奇异点，我们能做的不是消灭它，而是学会和它共处。就像开车遇到坑洼，减速通过，而不是把路填平。
本章小结：运动学约束是轨迹规划的“安全带”。关节位置、速度、加速度极限是底线，笛卡尔位姿约束是任务要求，奇异性规避是保命技能。三者缺一不可，而且相互耦合。处理得好，机器人稳如老狗；处理不好，分分钟给你颜色看。