控制

补一点 # 轨迹规划 ### (1) 轨迹： 机械手臂（的末端点或者操作点）的位置、速度、加速度对时间的历程；

可进一步定义成{T}相对{G}的状态历程（和手臂种类无关，{G}也可随时间变动，比如传送带）。 ### （2）理想轨迹: 机械手臂（的末端点或者操作点）的位置、速度、加速度对时间的历程； 理想轨迹，必须是Smooth Path(i.e.,continuous with continuous first derivative)。 Smooth Path不仅要求位置上连续，而且要求速度上也必须连续，若速度上不连续，速度曲线会有很多转折点，在转折点上，其加速度有可能突破限制（力学限制），连续速度曲线加速度变化不大，若速度变化不连续，在那一瞬间，执行器需要提供巨大扭力符合加速度要求，不符合真实的情况。

• via point，在轨迹规划过程中，往往还需要到达特定的中间位置，该位置叫做via point；
• initial point , 初始位置、姿态；
• final point，目标位置、姿态； 轨迹规划，常常在两个不同空间下进行处理，第一个是Joint Space，即将规划细化到每一个Joint上进行；另外一个是Cartesian Space上的轨迹规划，即当前知道手臂位置，直接在手臂末端点空间姿态上进行规划。

2. Joint Space下的轨迹规划

（1）步骤

• 定义{T} 相对于{G}的initial point, via, final point,  (包含移动和转动自由度）。

• i = 1: initial

• i = 2~N-1: via points

• i = N+1: final

• 注意，规划只有6个自由度，但是Trans Matrics有16个参数，故将其以6个参数方式来表达：

上面是移动，下面是转动

旋转矩阵里，9个数字有3个自由度，即6个限制条件，若各自规划9个数字，没有加以限制，有可能形成的矩阵很大几率不符合Rotation Matrix的形式，无法满足6个限制条件。所以我们直接以3个自由度形式进行规划。

• 做Inverse Kinematics: 将手臂末端点状态转换到joint状态： 
• 对所有joints规划smooth trajectories
• 做Forward Kinematics：将joint状态转换到手臂末端点状态，检查末端点在Cartesian Space下轨迹的可行性。

(2)示例

• joint space 
• Cartesian space 

经历了上述步骤，最后一步，进行Trajectory Checking... ，看看手臂在运动时，有没有碰倒障碍物，或者有没有满足其他的一些条件... ...

3. Cartesian-Space下的轨迹规划

（1）步骤

• 定义{T} 相对于{G}的initial point, via, final point,  (包含移动和转动自由度）。

• i = 1: initial

• i = 2~N-1: via points

• i = N+1: final

• 注意，规划只有6个自由度，但是Trans Matrics有16个参数，故将其以6个参数方式来表达：

• 在上述需求下，直接对所有手臂末端点规划smooth trajectories。

• 做Inverse Kinematics: 将规划好手臂末端点状态的轨迹点转换到joint状态： 

• 检查joint状态在Joint-Space下轨迹的可行性（有没有达到执行器的极限位置...blabla）

• COMMENTS

• Cartersian 方法用的比较多，因为其可以规划更具物理直观意义的轨迹。

• 缺点: 较高的运算负载(IK)，规划完了做IK。

• 比如已知init point，2个via points，1个final point，Joint Space的方法，只须做4次IK。

• 若4s空间，先在Cartesian下做规划，在做IK，运算量就大很多，比如规划出1000个路径，做IK，就需要算4000次。

• 对Real Time下，电脑的计算能力考验比较大。

(2) 示例

经历了上述步骤，最后一步，进行Trajectory Checking... ，检测各个Joint运动有没有超出限制... ...

机械臂运动规划中通常涉及几个坐标系。

坐标系	含义	常用记号
基坐标系（Base Frame / {G}）	机械臂底座固定的参考坐标系	{G}、{B}
工具坐标系（Tool Frame / {T}）	定义在末端执行器上的坐标系（TCP）	{T}
关节坐标系（Joint Frames）	每个关节都有自己的局部坐标系	{i}
任务坐标系 / 世界坐标系（Task / World Frame）	有时与基坐标系不同，用于描述外部环境或工件位置	{W}

6维表达（3个平移 + 3个旋转）

Joint Space 与 Cartesian Space 后面的笛卡尔坐标系就是末端执行器的6维表达（3个平移 + 3个旋转）；joint space可以说是每个电机的角度 它俩通过正/逆运动学可以互相转换，but有个问题：逆运动学会有多解 为什么： 机械臂的构型是冗余的（Redundant Manipulator）；有时不同解都能让末端到达同一位置（称为不同的“工作构型”）。 |维度|常见解的命名| |---|---| |肩部|左臂 / 右臂 (lefty/righty)| |肘部|肘上 / 肘下 (elbow up/down)| |腕部|翻转 / 不翻转 (wrist flip/non-flip)| 每个二元选择都可能导致 23=823=8 种不同的解。 还有一个问题奇异点（Singularities）。 - 奇异点就是机械臂处于某种几何姿态时，某些方向的运动会“失效”或变得不稳定；- 奇异点就是机械臂处于某种几何姿态时，某些方向的运动会“失效”或变得不稳定；

• 在数学上，对应雅可比矩阵 J(q)J(q) 的行列式为 0；

• 意义：此时关节的小变化不能引起末端的有效变化，系统自由度“退化”。

举例：

• 手臂完全伸直；

• 手腕三轴重叠；
  → 都是奇异点。

在奇异点附近：

• IK 解可能不连续；

• 很小的末端运动需要极大的关节运动；

• 控制器可能出现突跳或震荡。 多解其实还好，主要是奇异点

现在的逆运动学求解能否做到？改善这两个问题？

目前确实有不少逆运动学（IK）求解器（solver）表现不错、速度快、准确率高。下面我列出几个比较推荐的，并附上它们的优缺点（供你在选择时参考）。

---

✅ 推荐的 IK 求解器

1. IKPy

• 一个用 Python 写的通用 IK 库。GitHub

• 特点：支持任意机械臂链、支持 URDF、支持位置和姿态两者求解。GitHub

• 性能：作者表示“从 ~7 ms 到 ~50 ms”完成一次 IK 求解（取决于模型复杂度）GitHub

• 适合：快速原型、小型机械臂、教学或研究。

• 注意：如果用于工业实时控制（硬实时要求、低延迟、高频率）可能还需评估其 Python 实现的延迟与稳定性。

---

2. pick_ik （适用于 MoveIt 生态）

• 一个为 ROS/MoveIt 设计的 IK 求解插件。GitHub

• 特点：结合了局部优化（梯度下降等）+ 全局优化（例如进化算法）以解决冗余、多解、约束问题。index.ros.org

• 优点：在 ROS 机械臂系统中使用方便，可定制成本函数（例如最小化关节移动、避免关节极限、避免碰撞）GitHub

• 适合：使用 ROS/MoveIt 框架、需要集成规划器、碰撞检测、约束处理的系统。

---

3. IK‑Geo

• 一个更偏“几何解析”方向、C++/Python 混合实现的 IK 库。GitHub

• 特点：支持“分解子问题”（Paden–Kahan 子问题）等几何方法来求解 6R 或通用串联机械臂的 IK。

• 优点：对于结构良好的工业机械臂（例如腕部三轴交叉、肩肘结构良好）可能能够得到 解析解 或 接近解析解，速度快、精度高。

• 适合：你若有控制自己机械臂的几何结构、想要自行整合或实现高性能 IK 的话。

---

4. IKFast（来自 OpenRAVE）

• IKFast 是一个“生成器”（compiler）型工具，可以为给定机械臂生成专用的 C++ IK 求解器。维基百科+1

• 特点：生成后的求解器为专机型、解析或近解析方法、执行速度极快（有论文中提到几 µs 级别）维基百科

• 优点：如果你用于工业控制、需要最高性能、且机械臂结构已知固定，那它是一个非常好的选择。

• 缺点：配置和生成过程较为复杂；每更换机械臂结构或关节数都可能要重新生成。

---

⚠️ 选择时需要关注的 “因素”

• 结构匹配性：机械臂结构如果是标准的 “3‐轴腕部” + “肩肘部”结构（如很多工业 6 DoF 机械臂），则解析/几何方法（如 IK-Geo、IKFast）效果最佳。若结构非常不规则或冗余多，则可能需要数值方法。

• 实时性能：如果规划/控制频率要求很高（如 1 kHz 以上），要优先考虑生成型或 C++ 实现的求解器，而不是纯 Python 的。

• 约束处理能力：是否要考虑关节限位、碰撞检测、最小路径、避障、冗余优化等。许多“简单”求解器只求一个基本解，不处理这些。

• 稳定性与收敛性：数值方法可能陷入局部最优、收敛慢、或在奇异点附近表现差。解析方法虽然快，但可能受限于机械结构是否满足可解析条件。

• 集成便利度：是否与你系统（ROS/MoveIt、工业控制器、自定义控制系统）兼容。

• 维护和社区支持：库是否活跃、文档是否完善、是否有实际工程案例。

看这个视频# Hardware Demo of a Digital PID Controller 转载自Youtube：@Gregory L. Holst The demonstration in this video will show you the effect of proportional, derivative, and integral control on a real system. It's a DC motor connected to an optical rotary encod https://www.bilibili.com/video/BV1ix411M7RX/

PID 控制器为什么要用比例、积分、微分？

这个问题问得非常好，本质上是在问：为什么这三项刚好能解决“控制误差”的主要问题。我们可以从“误差如何表现”这个角度来理解，而不是从公式出发。

---

一、控制的核心问题是什么？

任何反馈控制，本质上都在处理一个量：

误差
e(t)=期望值−实际值e(t) = - e(t)=期望值−实际值

误差有三种典型“表现形式”：

1. 现在有多大（当前偏差）

2. 过去积累了多少（长期偏差）

3. 将来可能怎么变（变化趋势）

PID 正好一一对应这三类信息。

---

二、比例（P）：解决“现在差多少”

作用

比例项直接根据当前误差大小给控制输出：

uP=KP⋅e(t)u_P = K_P e(t)uP​=KP​⋅e(t)

为什么需要？

• 误差大 → 动作大

• 误差小 → 动作小

这是最直觉、最基本的控制方式。

但它解决不了什么？

• 稳态误差：
  在有负载、摩擦、偏置的系统中，单靠 P，往往需要“留一点误差”才能产生足够的输出

• KPK_PKP​ 太大 → 振荡、超调、不稳定

• KPK_PKP​ 太小 → 反应迟钝

👉 P 只能“对症”，不能“根治”

---

三、积分（I）：解决“为什么老是差一点”

作用

积分项累加历史误差：

uI=KI∫e(t) dtu_I = K_I e(t),dtuI​=KI​∫e(t)dt

为什么需要？

如果系统长期存在一个小误差：

• P 项输出是常数

• 但这个常数可能不足以抵消外界扰动（重力、负载）

积分会：

• 不断累加误差

• 直到误差被“逼到 0”

👉 I 的核心作用：消除稳态误差

但它带来什么问题？

• 响应变慢

• 容易 超调

• 执行器饱和时会发生 积分饱和（windup）

所以工程中常配合 anti-windup 使用。

---

四、微分（D）：解决“马上要出事了”

作用

微分项看的是误差变化率：

uD=KD⋅de(t)dtu_D = K_D uD​=KD​⋅dtde(t)​

为什么需要？

微分相当于在问：

“误差是不是正在快速变大？”

如果误差变化很快：

• D 项提前“踩刹车”

• 抑制超调和振荡

👉 D 的本质：预测趋势、增加阻尼

它像什么？

• 像减震器

• 像提前刹车，而不是撞上再修正

局限

• 对噪声非常敏感

• 实际中常对测量值微分 + 低通滤波

---

五、为什么一定是 P + I + D，而不是别的？

从信息角度看：

项	利用的信息	解决的问题
P	当前误差	响应速度
I	历史误差	稳态误差
D	误差趋势	超调 / 振荡

它们共同覆盖了：

现在、过去、未来

在不需要系统精确模型的前提下，
PID 是最简单、最通用、最鲁棒的一阶控制结构。

---

六、一个直觉类比（很重要）

想象你在 倒车入库：

• P：离车位远就多打方向盘

• I：如果一直偏一点点，就慢慢修正回来

• D：发现方向偏得越来越快，提前反打

你并没有解微分方程，但你在用 PID。

---

七、一句话总结

PID 之所以用比例、积分、微分，是因为它们分别对应误差的“大小、积累和变化趋势”，用最少的结构解决了速度、精度和稳定性三大矛盾。

windup（积分饱和/积分过冲）：

积分项“积累得太多”，即使误差变小，控制器也还在努力“修正过去的误差”，导致动作过大、系统超调或振荡。

anti-windup 的核心思想

防止积分累积过头，让系统更快恢复正常。

方法简单说：

1. 检测控制输出饱和（比如电压达到最大/最小）

2. 停止或减缓积分累积

   • 不让积分项继续“胡乱加”

3. 当误差回到合理范围时，积分再继续