跳转至

速度规划模块 - 理论文档

本文档快速讲解速度规划的理论基础、数学推导和核心算法原理。

目录

  1. 概述
  2. 梯形速度规划
  3. 余弦速度规划
  4. S形速度规划
  5. 微分追踪器理论
  6. 对比分析
  7. 参考文献

概述

什么是速度规划

速度规划是机器人运动控制的核心问题之一，其目标是在满足速度、加速度等约束条件下，生成一条平滑可行的速度曲线，使机器人从起点运动到终点。

为什么需要速度规划

直接对目标位置进行比例控制会导致： - 运动初期速度过快，造成冲击 - 接近目标时容易超调 - 运动不平滑，机械磨损加剧

图片描述：有无速度规划的对比图 - 上半部分（无速度规划）：位置响应曲线快速上升，超过目标值后震荡，标注"超调"和"震荡" - 下半部分（有速度规划）：位置响应曲线平滑上升，缓慢接近目标值，无超调 - 右侧对比说明框：列出无规划的缺点（冲击、超调、磨损）和有规划的优点（平滑、稳定、低磨损）

速度规划的基本约束

在实际应用中，速度规划必须满足以下物理约束：

\[0 \leq v(t) \leq v_{max}$$ $$|a(t)| \leq a_{max}$$ $$|j(t)| \leq j_{max}\]

其中： - \(v(t)\)：时刻 \(t\) 的速度 - \(a(t)\)：时刻 \(t\) 的加速度（速度的导数） - \(j(t)\)：时刻 \(t\) 的加加速度（加速度的导数，又称Jerk） - \(v_{max}\)：最大速度限制 - \(a_{max}\)：最大加速度限制 - \(j_{max}\)：最大加加速度限制

梯形速度规划

如果你想深入了解速度规划，这里是入门

算法原理

梯形速度规划是最基本的速度规划方法，其速度曲线由三段直线组成，呈现梯形形状。

图片描述：梯形速度规划的速度-时间曲线 - 横轴为时间 \(t\)，纵轴为速度 \(v\) - 曲线分为三个阶段： 1. 加速段（0 ~ \(t_1\)）：直线上升，斜率为正加速度 \(a_{max}\) 2. 匀速段（\(t_1\) ~ \(t_2\)）：水平直线，速度保持 \(v_{max}\) 3. 减速段（\(t_2\) ~ \(t_3\)）：直线下降，斜率为负加速度 \(-a_{max}\) - 标注各阶段的时间：加速时间 \(t_a = t_1 - t_0\)，匀速时间 \(t_c = t_2 - t_1\)，减速时间 \(t_d = t_3 - t_2\) - 总时间 \(T = t_a + t_c + t_d\) - 终点速度 \(v(t_3) = 0\)，起点速度 \(v(t_0) = 0\)

数学推导

设总位移为 \(D\)，最大速度为 \(V_{max}\)，最大加速度为 \(A_{max}\)。

步骤1：计算理论最小位移

若以最大加速度加速到 \(V_{max}\) 后立即减速至0，所需位移：

\[D_{accel\_decel} = \frac{V_{max}^2}{A_{max}} + \frac{V_{max}^2}{A_{max}} = \frac{2V_{max}^2}{A_{max}}\]

图片描述：三角形速度曲线（无匀速段） - 速度曲线呈三角形，起始和结束速度为0，顶点速度为 \(V_{max}\) - 加速时间和减速时间相等：\(t_a = t_d = \frac{V_{max}}{A_{max}}\) - 总位移为三角形面积：\(D = \frac{1}{2} \times T \times V_{max} = V_{max} \times \frac{V_{max}}{A_{max}} = \frac{V_{max}^2}{A_{max}}\) - 实际总位移应为两个三角形面积之和（加速+减速）：\(D_{total} = 2 \times \frac{V_{max}^2}{A_{max}} = \frac{2V_{max}^2}{A_{max}}\)

步骤2：判断是否达到最大速度

比较 \(D\) 与 \(D_{accel\_decel}\)：

  • 情况1：\(D \leq D_{accel\_decel}\)（距离太短，无法达到最大速度）
    • 实际最大速度 \(V_{peak} = \sqrt{A_{max} \times D}\)
    • 加速时间 \(t_a = \frac{V_{peak}}{A_{max}}\)
    • 减速时间 \(t_d = t_a\)（对称三角形）
    • 无匀速段
  • 情况2：\(D > D_{accel\_decel}\)（距离足够，可达到最大速度）
    • 实际最大速度 \(V_{peak} = V_{max}\)
    • 加速时间 \(t_a = \frac{V_{max}}{A_{max}}\)
    • 减速时间 \(t_d = \frac{V_{max}}{A_{max}}\)
    • 匀速段时间 \(t_c = \frac{D - D_{accel\_decel}}{V_{max}}\)

图片描述：两种情况的对比 - 左图（情况1，三角形）：速度曲线只有加速段和减速段，无匀速段，最大速度 \(V_{peak} < V_{max}\) - 右图（情况2，梯形）：速度曲线包含加速段、匀速段、减速段三部分，达到最大速度 \(V_{max}\) - 下方的位移对比图：三角形面积小于梯形面积

步骤3：生成速度曲线

对于每时刻 \(t\)，计算对应速度：

\[ v(t) = \begin{cases} A_{max} \cdot t & 0 \leq t < t_a \\ V_{max} & t_a \leq t < t_a + t_c \\ V_{max} - A_{max} \cdot (t - t_a - t_c) & t_a + t_c \leq t < T \end{cases} \]

算法实现流程图

图片描述：梯形速度规划算法流程图 - 流程图展示以下步骤： 1. 开始 2. 输入：\(D\)（目标位移），\(V_{max}\)（最大速度），\(A_{max}\)（最大加速度） 3. 计算 \(D_{accel\_decel} = 2V_{max}^2 / A_{max}\) 4. 判断：\(D \leq D_{accel\_decel}\)？ - 是 → 计算 \(V_{peak} = \sqrt{A_{max} \times D}\)，\(t_a = V_{peak} / A_{max}\)，\(t_c = 0\) - 否 → 计算 \(t_a = V_{max} / A_{max}\)，\(t_c = (D - D_{accel\_decel}) / V_{max}\) 5. 计算总时间 \(T = 2t_a + t_c\) 6. 判断当前时间 \(t\) 所在阶段 - 加速段：\(v = A_{max} \cdot t\) - 匀速段：\(v = V_{max}\) - 减速段：\(v = V_{max} - A_{max} \cdot (t - t_a - t_c)\) 7. 输出速度 \(v(t)\) 8. 结束

优缺点分析

优点： - 实现简单，计算量小 - 易于理解和调试 - 适用于大多数工业场景

缺点： - 加速度突变，存在机械冲击 - 高速运行时舒适性和平稳性较差 - 不适合对冲击敏感的系统（如精密仪器、医疗设备）

余弦速度规划

算法原理

余弦速度规划使用余弦函数生成速度曲线，避免梯形规划中加速度突变的问题。

图片描述：余弦速度规划的速度-时间曲线 - 横轴为时间 \(t\)，纵轴为速度 \(v\) - 速度曲线呈余弦波形：从0开始，上升到 \(V_{max}\)，再下降到0 - 曲线方程：\(v(t) = \frac{V_{max}}{2} \left[1 - \cos\left(\frac{2\pi t}{T}\right)\right]\) - 标注关键点：起点 \((0, 0)\)，中点 \((T/2, V_{max})\)，终点 \((T, 0)\) - 曲线平滑，无尖角

数学推导

步骤1：定义速度函数

为使速度曲线从0开始、0结束，且最大值不超过 \(V_{max}\)，采用：

\[v(t) = \frac{V_{max}}{2} \left[1 - \cos\left(\frac{2\pi t}{T}\right)\right]\]

其中 \(T\) 为总运动时间。

步骤2：验证边界条件

  • 起点：\(v(0) = \frac{V_{max}}{2} [1 - \cos(0)] = \frac{V_{max}}{2} [1 - 1] = 0\) ✓
  • 终点：\(v(T) = \frac{V_{max}}{2} [1 - \cos(2\pi)] = \frac{V_{max}}{2} [1 - 1] = 0\) ✓
  • 中点：\(v(T/2) = \frac{V_{max}}{2} [1 - \cos(\pi)] = \frac{V_{max}}{2} [1 - (-1)] = V_{max}\) ✓

步骤3：计算加速度

加速度是速度的导数：

\[a(t) = \frac{dv}{dt} = \frac{V_{max}}{2} \cdot \frac{2\pi}{T} \sin\left(\frac{2\pi t}{T}\right) = \frac{\pi V_{max}}{T} \sin\left(\frac{2\pi t}{T}\right)\]

图片描述：余弦规划的加速度-时间曲线 - 横轴为时间 \(t\)，纵轴为加速度 \(a\) - 加速度曲线呈正弦波形 - 起点加速度 \(a(0) = 0\)，避免初始冲击 - 中点加速度 \(a(T/2) = \frac{\pi V_{max}}{T}\)（最大加速度） - 终点加速度 \(a(T) = 0\)，平滑停止 - 曲线平滑连续，无突变

步骤4：计算位移

位移是速度的积分：

\[x(t) = \int_0^t v(\tau) d\tau = \frac{V_{max}}{2} \int_0^t \left[1 - \cos\left(\frac{2\pi \tau}{T}\right)\right] d\tau$$ $$x(t) = \frac{V_{max}}{2} \left[t - \frac{T}{2\pi} \sin\left(\frac{2\pi t}{T}\right)\right]\]

图片描述：余弦规划的位移-时间曲线 - 横轴为时间 \(t\)，纵轴为位移 \(x\) - 位移曲线呈S形，起始和结束处斜率（速度）为0 - 曲线关于中点 \((T/2, D/2)\) 中心对称 - 终点位移 \(x(T) = \frac{V_{max}}{2} [T - 0] = \frac{V_{max}T}{2}\) - 由于 \(\int_0^T \cos(2\pi t/T) dt = 0\)，总位移 \(D = \frac{V_{max}T}{2}\)

总时间计算

已知目标位移 \(D\)，求解总时间 \(T\)：

\[D = \frac{V_{max}T}{2} \implies T = \frac{2D}{V_{max}}\]

但此公式未考虑加速度约束。实际应用中需要验证：

\[a_{max\_required} = \frac{\pi V_{max}}{T} = \frac{\pi V_{max}^2}{2D}\]

若 \(a_{max\_required} \leq A_{max}\)，则上述公式成立；否则需要延长总时间 \(T\)。

优缺点分析

优点： - 加速度连续，机械冲击小 - 运动平滑，舒适性好 - 实现简单，无需迭代计算

缺点： - 加速度变化率（Jerk）不均匀 - 最高加速度出现在运动中点 - 无法独立控制最大加速度

图片描述：梯形与余弦规划对比 - 同一坐标系中绘制两条曲线：梯形（虚线）和余弦（实线） - 梯形曲线：加速段斜率恒定（矩形加速度波），匀速段水平，减速段对称 - 余弦曲线：平滑上升和下降，正弦加速度波 - 下方对比表：列出两种方法在加速度连续性、计算复杂度、冲击大小等维度的对比

S形速度规划

（本节框架已预留，内容待补充）

概述

S形速度规划（又称七段式速度规划）通过控制加加速度（Jerk），实现更平滑的运动。其速度曲线呈S形，故得名。

核心思想

S形规划将整个运动过程分为7个阶段： 1. 匀加速段（Jerk = \(J_{max}\)） 2. 变加速段（Jerk线性减小到0，加速度恒定） 3. 匀减速段（Jerk = \(-J_{max}\)） 4. 匀速段 5. 匀减速段 6. 变减速段 7. 匀加速段

关键公式

（请在此处补充S形速度规划的相关公式推导）

速度曲线

（请在此处补充S形速度规划的速度-时间、加速度-时间、Jerk-时间曲线图）

实现要点

（请在此处补充S形速度规划的实现注意事项）

微分追踪器理论

理论背景

在控制系统中，直接对输入信号进行微分会放大噪声，导致控制效果恶化。传统做法是使用低通滤波器，但这会引入相位延迟。

微分追踪器（Differentiator）是自抗扰控制（ADRC）的核心组件，通过安排过渡过程（Transition Process），在不放大噪声的前提下获得输入信号的微分估计。

核心思想

微分追踪器的设计目标： 1. 大误差时：快速响应，"油门焊死"追踪目标 2. 接近目标时：平滑过渡，避免超调和震荡

图片描述：微分追踪器的工作原理 - 上图：传统微分器与微分追踪器的对比 - 传统微分器：输入信号突变时输出尖峰脉冲（噪声放大） - 微分追踪器：输入信号突变时输出平滑的过渡过程 - 下图：微分追踪器的输入-输出关系 - 输入：阶跃信号（目标值） - 输出：平滑的跟踪信号及其微分

数学模型

微分追踪器通过以下状态方程实现：

\[ \begin{cases} \dot{x}_1 = x_2 \\ \dot{x}_2 = x_3 \\ \dot{x}_3 = R^2 \cdot fhan(e, x_2, R, h) \end{cases} \]

其中： - \(x_1\)：对输入信号的跟踪估计 - \(x_2\)：输入信号的微分估计 - \(x_3\)：内部状态变量 - \(R\)：跟踪速度因子（越大响应越快） - \(h\)：滤波因子（越大滤波效果越好） - \(e\)：跟踪误差，\(e = v_0 - x_1\)（\(v_0\) 为输入信号）

fhan函数详解

fhan（Fastest Handian Nonlinear Function，最速控制函数）是微分追踪器的核心非线性函数：

\[ fhan = -R \cdot \text{sign}(d) \cdot \begin{cases} \frac{a_0}{d} & |d| > \delta \\ \frac{a_0 + d}{2} \cdot \frac{1}{h} & |d| \leq \delta \end{cases} \]

其中： $\(d = R \cdot h^2\)$ $\(a_0 = h \cdot x_2 + x_1\)$ $\(a_1 = \sqrt{d \cdot (R \cdot h + 0.5 \cdot |x_1| \cdot h + |x_2| \cdot h^2)}\)$ $\(a = \frac{a_0 + \text{sign}(a_1) \cdot a_1 + x_1}{2}\)$ $\(\delta = R \cdot h\)$

图片描述：fhan函数的结构图 - 输入：\(e\)（误差）、\(\dot{e}\)（误差微分）、\(R\)（速度因子）、\(h\)（滤波因子） - 计算中间变量：\(d\)、\(a_0\)、\(a_1\)、\(a\)、\(\delta\) - 条件判断：\(|d| > \delta\) 还是 \(|d| \leq \delta\) - 输出：\(fhan\) 值 - 标注关键阈值：\(\delta = R \cdot h\)

图片描述：fhan函数的输出特性 - 横轴：输入误差 \(e\) - 纵轴：输出 \(fhan\) - 曲线分为两个区域： - 线性区（\(|e| < \delta\)）：输出与输入近似线性关系，平滑过渡 - 饱和区（\(|e| > \delta\)）：输出饱和，趋向最快速响应 - 标注饱和值：\(\pm R\)

fhan函数的作用机理

当误差较大时（\(|e| > \delta\)）：

\[fhan \approx -R \cdot \text{sign}(e)\]

系统以最大速度 \(R\) 快速逼近目标，体现"油门焊死"的特性。

当误差较小且速度较大时（\(|e| \leq \delta\) 但 \(|\dot{e}|\) 较大）：

\[fhan \approx -\frac{R}{h} \cdot \dot{e}\]

引入阻尼项，抑制速度过快，防止超调。

当接近目标时（\(|e| \leq \delta\) 且 \(|\dot{e}|\) 较小）：

\[fhan \approx -\frac{R}{h^2} \cdot e\]

退化为PD控制，实现平滑收敛。

图片描述：fhan函数的三段特性 - 区域1（大误差）：饱和输出 \(-R\)，最快速响应 - 区域2（中等误差）：线性过渡，阻尼作用 - 区域3（小误差）：PD控制，平滑收敛 - 标注各区域的误差范围和输出特性

微分追踪器的工作流程

图片描述：微分追踪器算法流程图 - 输入：目标值 \(v_0\)，测量值 \(y\)，时间间隔 \(dt\) - 步骤1：计算跟踪误差 \(e = v_0 - x_1\) - 步骤2：计算 \(d = R \cdot h^2\) - 步骤3：计算 \(a_0 = h \cdot x_2 + x_1\) - 步骤4：计算 \(a_1 = \sqrt{d \cdot (R \cdot h + 0.5 \cdot |x_1| \cdot h + |x_2| \cdot h^2)}\) - 步骤5：计算 \(a = \frac{a_0 + \text{sign}(a_1) \cdot a_1 + x_1}{2}\) - 步骤6：计算 \(\delta = R \cdot h\) - 步骤7：条件判断 - 若 \(|d| > \delta\)：\(fhan = -R \cdot \text{sign}(a - x_1)\) - 若 \(|d| \leq \delta\)：\(fhan = -R \cdot \frac{a - x_1}{h}\) - 步骤8：更新状态 - \(x_3 = fhan\) - \(x_2 = x_2 + x_3 \cdot dt\) - \(x_1 = x_1 + x_2 \cdot dt\) - 输出：\(x_2\)（微分估计），\(x_1\)（跟踪值）

参数分析

参数 \(R\)（跟踪速度因子）：

  • 物理意义：系统响应的最大加速度
  • 增大 \(R\)：响应更快，但噪声放大
  • 减小 \(R\)：系统更平滑，但响应变慢
  • 建议范围：\(10^4 \sim 10^7\)（根据系统特性调整）

参数 \(h\)（滤波因子）：

  • 物理意义：相当于滤波时间常数
  • 增大 \(h\)：滤波效果增强，延迟增加
  • 减小 \(h\)：响应更快，但噪声敏感
  • 建议范围：\(0.001 \sim 0.1\) 秒

参数 \(h_0\)（初始滤波因子）：

  • 用于首次计算，避免初始冲击
  • 通常设为与 \(h\) 相同

图片描述：参数 \(R\) 对响应的影响 - 同一坐标系中绘制三条曲线 - \(R_1\) 较小：响应慢，平滑 - \(R_2\) 中等：平衡响应速度和噪声 - \(R_3\) 较大：响应快，但有轻微震荡 - 标注各曲线的上升时间和超调量

图片描述：参数 \(h\) 对噪声的影响 - 上图：不同 \(h\) 值下的跟踪效果 - \(h\) 较小：跟踪快，但噪声明显 - \(h\) 较大：跟踪平滑，但有延迟 - 下图：对应的微分输出 - \(h\) 较小时：微分输出噪声大 - \(h\) 较大时：微分输出平滑

与传统方法的对比

特性 传统微分器 高通滤波 微分追踪器
噪声放大 严重 较小 可控
相位延迟 无 较大 较小
计算复杂度 低 低 中等
参数调节 无 简单 需要经验
适用场景 低噪声环境 一般场景 高噪声场景

图片描述：三种方法的阶跃响应对比 - 输入：单位阶跃信号 - 曲线1（传统微分器）：输出尖峰脉冲，幅值趋向无穷大 - 曲线2（高通滤波）：输出有延迟，且有稳态误差 - 曲线3（微分追踪器）：输出平滑跟踪，无超调，无延迟 - 下方标注：各方法的上升时间、超调量、稳态误差

应用场景

微分追踪器适用于： 1. 需要获取速度/加速度估计的场合 2. 输入信号存在噪声或跳变的系统 3. 对控制平滑性要求高的场景 4. 作为自抗扰控制器（ADRC）的核心组件

对比分析

三种速度规划方法对比

特性 梯形 余弦 S形
加速度连续性 突变 连续 连续
Jerk连续性 突变 突变 连续
计算复杂度 低 低 中
实现难度 易 易 难
运动平滑性 一般 较好 最好
适用场景 工业机器人 一般运动 精密运动

图片描述：三种方法的速度-加速度对比 - 三列图：梯形、余弦、S形 - 每列两行：上图为速度-时间曲线，下图为加速度-时间曲线 - 梯形：加速度矩形波，有突变 - 余弦：加速度正弦波，连续但不平滑 - S形：加速度平滑曲线，完全连续

速度规划与微分追踪器的对比

特性 速度规划 微分追踪器
目标 生成可行轨迹 估计信号微分
输入 起终点位置 任意输入信号
输出 速度曲线 微分估计值
约束处理 显式处理 隐式处理
典型应用 轨迹生成 状态估计

图片描述：两种方法的协同使用 - 流程图：目标位置 → 速度规划 → 速度指令 → 微分追踪器 → 速度/加速度估计 → 控制器 → 执行器 - 说明：速度规划生成参考轨迹，微分追踪器平滑处理参考信号并提供微分估计，两者配合实现精确控制

参考文献

  1. 韩京清. 自抗扰控制技术. 国防工业出版社, 2008.
  2. Gao, Z. "Active Disturbance Rejection Control: A Paradigm Shift in Feedback Control System Design." Proceedings of the 2006 American Control Conference, 2006.
  3. Lynch, K. M., & Park, F. C. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press, 2017.
  4. Biagiotti, L., & Melchiorri, C. Trajectory Planning for Automatic Machines and Robots. Springer, 2008.
  5. 速度规划(梯形、余弦、多项式、7段S型、7段修正S型、15段S型、31段S型)-CSDN博客 部分图片出处

文档版本：v1.0 最后更新：2024-XX-XX

评论