机器人轨迹规划入门：把 q 当成坐标系，彻底搞懂 q、s、t、方向和速度

机器人轨迹规划初学时，最容易混淆的几个概念是：

q 到底是什么？
q 是角度，还是位置？
q(t) 和 q(s) 有什么区别？
dir 是不是速度？
s 是不是时间？
Lookahead 和 S 曲线到底在规划什么？

这些概念之所以容易混，是因为机器人同时存在关节空间、笛卡尔空间、时间、路径几个不同层次。

其实只要先建立一个非常简单的类比，后面就容易了：

把机器人所有关节变量 q，看成一个坐标系中的坐标。

一、首先搞清楚：q 到底是什么？

对于一个六轴机械臂，可以写：

$$ q=(q_1,q_2,q_3,q_4,q_5,q_6) $$

这里的每一个 (q_i)，就是一个关节变量。

如果这个关节是旋转关节：

$$ q_i=\text{关节角度} $$

例如：

$$ q_1=30^\circ $$

表示第1个关节转到了30°。

如果是移动关节：

$$ q_i=\text{关节位移} $$

例如：

$$ q_1=200mm $$

表示这个移动关节移动到了200 mm。

所以对于常见的六轴工业机器人：

q 通常就是六个关节角度组成的向量。

例如：

$$ q=(30^\circ,20^\circ,-10^\circ,40^\circ,0^\circ,15^\circ) $$

这代表机器人当前的一个关节姿态。

二、最重要的理解：q 本身就是一个“坐标”

这一点非常重要。

很多初学者看到：

$$ q=(q_1,q_2) $$

会觉得这是一个很抽象的东西。

其实一点都不抽象。

假设机器人只有两个旋转关节：

$$ q=(q_1,q_2) $$

那么完全可以把它画成一个普通的二维坐标系：

q2（角度）
↑
│
│                 ●
│              ／
│           ／
│        ／
│     ●
│
└────────────────────→ q1（角度）

这和我们平常画：

y
↑
│             ●
│          ／
│       ／
│    ●
│
└────────────────→ x

本质上是一样的。

只不过：

普通数学：
(x, y)

机器人关节空间：
(q1, q2)

最大的区别只是：

x、y 是普通空间的位置坐标；q1、q2 是机器人各个关节的变量，通常是角度。

三、所以什么叫“关节空间”？

现在就很好理解了。

如果机器人只有两个关节：

$$ q=(q_1,q_2) $$

那么：

$$ (q_1,q_2) $$

就构成一个二维的关节空间。

例如：

q2
↑
│
│       ● A
│      /
│     /
│    ● B
│
└──────────────→ q1

A 点可能表示：

$$ q_A=(60^\circ,30^\circ) $$

B 点可能表示：

$$ q_B=(20^\circ,10^\circ) $$

这不是说机械臂末端在空间中的坐标是 (60,30)。

而是说：

机器人两个关节分别处于60°和30°。

所以一定要区分：

笛卡尔空间

例如：

$$ (x,y,z) $$

描述：

机械臂末端在真实三维空间中的位置。

关节空间

例如：

$$ (q_1,q_2,q_3,\dots) $$

描述：

每个关节分别转到了什么角度。

四、二维情况下，q 就是一条普通的二维曲线

这也是理解 dir 最关键的一步。

假设：

$$ q=(q_1,q_2) $$

机器人从：

$$ q_A=(10^\circ,20^\circ) $$

运动到：

$$ q_B=(50^\circ,60^\circ) $$

那么在 q1-q2 坐标系中，就是：

q2
↑
│
│                    B
│                  ●
│               ／
│            ／
│         ／
│      ●
│      A
│
└────────────────────────→ q1

这就是：

机器人在关节空间中的运动轨迹。

如果中间的运动更加复杂：

q2
↑
│
│              ●
│           ／   ＼
│        ／         ●
│     ●
│
└────────────────────────→ q1

那么这就是一条曲线。

所以：

当 q 只有两个变量时，机器人轨迹完全可以理解成 q1-q2 平面上的一条曲线。

这比直接说“q 是一个抽象状态向量”更容易理解。

五、q 到底是“一个点”还是“一条曲线”？

两种说法都对，但要看你说的是：

$$ q $$

还是：

$$ q(s) $$

q 是一个点

例如：

$$ q=(30^\circ,20^\circ) $$

它代表机器人当前的一个姿态。

在二维关节空间里就是：

q2
↑
│
│        ● q
│
└────────────→ q1

q(s) 是一条曲线

当机器人运动时，q 会不断变化：

$$ q=q(s) $$

于是：

q2
↑
│
│             ●
│          ／
│       ／
│    ●
│
└────────────────→ q1

这就是一条轨迹。

所以可以记住：

q 是一个点，q(s) 是一条轨迹。

六、那么 s 是什么？

s 可以理解成：

沿着这条曲线走了多远。

例如：

q2
↑
│
│                 ●
│              ／
│           ／
│        ●
│     ／
│  ●
│
└────────────────────→ q1
    ↑        ↑       ↑
   s=0      s=50    s=100

这里的 s 是路径参数。

如果我们规定 s 就是曲线长度，那么：

$$ s=0 $$

是起点，

$$ s=50 $$

是沿着轨迹走了50个长度单位，

$$ s=100 $$

是走了100个长度单位。

因此：

$$ q=q(s) $$

就是：

走到轨迹的哪个位置时，q1、q2……分别是多少。

七、这时候再理解“切线方向”就非常简单

二维情况下：

q2
↑
│
│             ●
│           ／
│         ／
│       ／ ← 切线
│     ●
│
└────────────────→ q1

这和普通数学曲线的切线完全一样。

如果：

$$ q=(q_1,q_2) $$

那么曲线的切线方向来自：

$$ \frac{dq}{ds} = \left( \frac{dq_1}{ds}, \frac{dq_2}{ds} \right) $$

归一化之后：

$$ dir= \frac{dq/ds}{|dq/ds|} $$

就是：

机器人在关节空间这条曲线上，此刻前进的单位方向。

所以 dir 没有什么神秘的。

它就是：

q1-q2 坐标系中，这条曲线的切线方向。

八、六轴以后只是从二维变成六维

如果：

$$ q=(q_1,q_2) $$

是二维：

(q1,q2)
↓
二维平面

那么：

$$ q=(q_1,q_2,q_3) $$

就是三维关节空间。

而六轴机器人：

$$ q=(q_1,q_2,q_3,q_4,q_5,q_6) $$

就是六维关节空间。

虽然我们没法直接把六维空间画出来，但数学关系完全一样：

$$ dir= \frac{dq/ds}{|dq/ds|} $$

所以不要因为“六维空间画不出来”就把它想得很复杂。

可以一直类比：

2个关节：

(q1,q2)
≈
(x,y)


3个关节：

(q1,q2,q3)
≈
(x,y,z)


6个关节：

(q1,q2,q3,q4,q5,q6)
≈
六维坐标空间

只是坐标的物理意义变成了各个关节的角度或位移。

九、q(s) 和 q(t) 到底有什么区别？

现在有了关节空间的概念，就很好理解了。

q(s)

表示：

沿着关节空间轨迹走到某个位置时，机器人是什么姿态。

它描述的是：

轨迹的形状。

q(t)

表示：

经过某个时间以后，机器人是什么姿态。

它描述的是：

实际运动过程。

两者通过：

$$ s=s(t) $$

联系起来：

$$ \boxed{q(t)=q(s(t))} $$

这句话可以翻译成：

先确定轨迹上的位置 s，再根据时间决定现在走到 s 的哪里。

十、用汽车来类比

假设汽车已经规划好一条弯曲的道路。

道路：

$$ q(s) $$

表示道路本身。

s：

沿着道路走了多少。

t：

开车用了多少时间。

那么：

$$ s(t) $$

就是：

每个时间点汽车走到道路的什么位置。

最后：

$$ q(t)=q(s(t)) $$

就是：

每个时间点汽车到底在哪里。

机器人完全一样。

十一、dir 和速度千万不要混

现在有一个非常重要的区别。

假设：

$$ dir=(0.8,-0.6) $$

这只是：

在 q1-q2 空间中往哪个方向走。

它并没有告诉你走多快。

如果路径速度是：

$$ v=100 $$

那么才可以计算实际的关节速度。

最简单情况下：

$$ \dot q=dir\cdot v $$

于是：

$$ \dot q=(80,-60) $$

也就是说：

dir
↓
往哪里走

v
↓
走多快

两者结合
↓
实际关节速度

十二、为什么代码里还有 dir_scale？

因为你的“路径”不一定是在关节空间里定义的。

例如：

关节空间轨迹

路径长度直接用关节空间长度表示：

$$ s_q $$

这时可以近似：

$$ \dot q=dir\cdot v $$

笛卡尔空间轨迹

例如机械臂末端沿直线移动：

末端：
A ───────────→ B

路径速度可能是：

$$ v=100mm/s $$

但关节速度是：

$$ rad/s $$

所以需要一个比例：

$$ dir\_scale $$

于是代码里：

$$ \boxed{ \dot q=dir\cdot dir\_scale\cdot v } $$

这里：

dir 决定方向
dir_scale 负责空间尺度转换
v 决定沿路径走多快

十三、特别注意：末端直线 ≠ q 空间直线

这是机器人初学者另一个非常容易犯的错误。

假设我们要求：

机械臂末端从 A 点直线走到 B 点。

在笛卡尔空间里：

x
↑
│
│       B ●
│         │
│         │
│         │
│       A ●
└────────────────→

这是直线。

但是经过逆运动学以后：

$$ (x,y,z) \rightarrow (q_1,q_2,\dots,q_6) $$

在关节空间里可能变成：

q2
↑
│
│             ●
│          ／
│       ／
│     ●
│      ＼
│        ●
│
└────────────────→ q1

也就是说：

笛卡尔空间的轨迹和关节空间的轨迹不是同一条曲线。

所以看代码的时候，一定先问：

这里的 length、dir、v 到底属于哪个空间？

这是非常重要的工程习惯。

十四、dir_in 和 dir_out 就是曲线两端的切线

假设某一段轨迹：

q2
↑
│
│             ● 出口
│           ／
│         ／
│       ／
│     ● 入口
│
└────────────────→ q1

入口处的切线：

$$ dir_{in} $$

出口处的切线：

$$ dir_{out} $$

如果这一段是直线：

$$ dir_{in}=dir_{out} $$

如果这一段是弯曲的：

$$ dir_{in}\neq dir_{out} $$

所以：

dir_in 和 dir_out 本质上就是轨迹两端的运动方向。

十五、Lookahead 为什么要比较两个 dir？

假设：

上一段：

────────────→


下一段：

             ↘
              ↘

在连接点发生了方向变化。

上一段出口：

$$ dir_{out} $$

下一段入口：

$$ dir_{in} $$

如果两者差别很大，说明拐弯很急。

这时如果速度还特别高，关节速度变化就可能太大。

所以 Lookahead 会限制：

$$ V $$

即这个拐角允许通过的速度。

于是：

方向变化越剧烈，通常允许的连接速度越低。

如果：

$$ dir_{out}=dir_{in} $$

说明两段完全同向，那么就没有因为方向突变产生的额外速度变化。

十六、Lookahead 和 S 曲线负责的事情不同

这一点也一定要分清。

Lookahead

解决：

这一段入口和出口最多允许多快？

得到：

$$ v_{entry} $$

和：

$$ v_{exit} $$

S 曲线

解决：

已经知道入口和出口速度以后，怎么平滑地从一个速度变成另一个速度？

它控制：

$$ v(t) $$

$$ a(t) $$

$$ j(t) $$

所以可以简单记：

Lookahead 决定“速度上限和连接速度”；S 曲线决定“速度怎么变化”。

十七、为什么 Lookahead 要前后各扫描一次？

假设有：

段1 → 段2 → 段3 → 终点

终点必须停下来：

$$ v=0 $$

那么：

段3不能跑太快，否则来不及停。

进一步：

段2也不能跑太快，否则段3来不及降速。

再进一步：

段1也受到影响。

这就是后向扫描：

终点
 ↓
段3
 ↓
段2
 ↓
段1

它解决：

未来要求我降下来，所以现在不能跑太快。

反过来：

如果起点速度是：

$$ v=0 $$

而段1很短，那么即使后面允许：

$$ v=100 $$

段1也不可能瞬间从0变成100。

所以还需要：

起点
 ↓
段1
 ↓
段2
 ↓
段3

这就是前向扫描。

它解决：

我现在速度不够高，未来也不能要求我瞬间跑那么快。

十八、最后再区分速度、加速度和 jerk

沿着路径：

$$ v=\frac{ds}{dt} $$

是路径速度。

表示：

每秒沿着轨迹走多远。

加速度：

$$ a=\frac{dv}{dt} $$

表示：

速度变化有多快。

jerk：

$$ j=\frac{da}{dt} $$

表示：

加速度变化有多快。

所以：

s
↓ 对时间求导
v
↓ 对时间求导
a
↓ 对时间求导
j

这就是 S 曲线规划的核心。

十九、把所有概念放到一张图里

现在可以把整个机器人轨迹规划过程理解成：

                 关节空间
                    │
                    ▼
            q=(q1,q2,...,q6)
              “一个坐标点”
                    │
                    │ 随着运动变化
                    ▼
                  q(s)
             “一条轨迹曲线”
                    │
                    ▼
              dq/ds / dir
             “曲线的切线方向”
                    │
                    ▼
               Lookahead
          “这一段最多跑多快”
                    │
             v_entry / v_exit
                    │
                    ▼
               S-Curve
       “速度怎样平滑地变化”
                    │
                    ▼
                  s(t)
          “时间对应轨迹位置”
                    │
                    ▼
                  q(t)
          “某个时间的机器人姿态”

最终可以写成：

$$ \boxed{ q=(q_1,q_2,\dots,q_n) } $$

是关节空间中的一个点。

然后：

$$ \boxed{q=q(s)} $$

是关节空间中的一条轨迹。

再通过：

$$ \boxed{s=s(t)} $$

得到：

$$ \boxed{q(t)=q(s(t))} $$

最终就得到了机器人随时间变化的实际运动。

二十、初学者最值得记住的 8 句话

① q 是什么？

q 是关节变量组成的坐标，旋转关节通常是角度，移动关节通常是位移。

② 什么是关节空间？

所有 q 变量组成的坐标空间。

③ 二关节机器人怎么理解？

q=(q1,q2)，就把它当成普通的 x-y 坐标系。

④ q 是一个点还是一条线？

q 是一个点；q(s) 是机器人在关节空间里运动形成的曲线。

⑤ dir 是什么？

dir 就是这条 q 空间曲线当前点的单位切线方向。

⑥ s 是什么？

s 是沿着轨迹走到哪里，不是时间。

⑦ t 是什么？

t 是时间，决定机器人什么时候走到轨迹上的某个 s。

⑧ Lookahead 和 S 曲线是什么？

Lookahead 决定“能跑多快”，S 曲线决定“怎么平滑地跑”。

最后一个非常重要的工程习惯

以后看到机器人轨迹代码里的任何一个变量，先问三个问题：

第一，它属于哪个空间？

是：

• 关节空间？
• 笛卡尔空间？

第二，它描述的是位置、方向还是速度？

是：

• q
• dir
• v
• a
• j

第三，它是关于什么变化的？

是：

• 关于路径 (s)？
• 关于时间 (t)？

只要这三个问题不混，机器人轨迹规划的大部分概念就不会乱。

最核心的关系其实非常简单：

$$ \boxed{ \text{q 是坐标} \rightarrow \text{q(s) 是轨迹} \rightarrow \text{dir 是轨迹切线方向} \rightarrow \text{v 决定沿轨迹走多快} \rightarrow \text{t 决定什么时候走到哪里} } $$

这就是理解 Lookahead 和 S 曲线之前最重要的基础。