Part I · 第 1 章线性代数 · 1.1

向量

机器人感知到的、发出去的几乎一切，都是一串带几何含义的数字：杯子在哪里，车跑多快，手爪受多大的力，机械臂六个关节各转了多少。这串数字就是向量。它同时是一支箭头、一张数字清单和空间里的一个点。能在这三种说法之间自由切换，是整门课的第一项本领。

约 20 分钟前置：高中数学 7 个交互

1为什么机器人需要它

相机看见桌上的杯子，报告："杯子在 (0.8, −0.2, 0.1) 米。"里程计说，这一秒车走了 (0.3, 0.05) 米。力传感器说，手爪受力 (0, 0, −4.9) 牛。机械臂控制器每个周期读到 6 个关节角。激光雷达每转一圈给出几千个点，每个点是一对 (x, y)。

这些东西看上去毫不相干，却有一个共同点：每一项都是一列数字，而且每个数字都有几何含义。数学上把它们统称为向量 (vector)。一旦承认这一点，就能用同一套操作去处理它们：两段位移相加得到总位移，把速度放大一倍，量两点之间的距离，给一个方向配上长度。后面整门课的数学，都是从这几个朴素的操作里长出来的。

这一节只做一件事：让"向量"从一个名词变成手感。

怎么玩这些页面

图中带彩色圆环的点是手柄，直接拖动就行。"试一试"里的任务做到了会自己打勾。右上角可以切换中文 / EN 和明暗主题。

2直觉：同一件事的三张脸

向量有三张脸，说的是同一件事：

箭头：有方向和长度，不在乎尾巴钉在哪里。把它平移（不转、不拉长）到任何地方，它仍然是同一个向量。
一串数字（分量 components）：沿 x 走多少，沿 y 走多少。竖着写成 $\begin{bmatrix}3\\2\end{bmatrix}$。在机器人代码里，它就是一个数组。
一个点：把尾巴钉在原点，箭头尖落在平面上的某一点，这个点的坐标就是向量的分量。

拖动下图：琥珀色箭头的尖端用来改变 $\cv{\mathbf v}$；青色小环抓着 $\cv{\mathbf v}$ 的一个副本的尾巴，可以把它滑到别处。看看数字变不变。

FIG 1三张脸

红、绿虚线是 $\mathbf v$ 沿 x、y 方向的两个分量。副本换了位置，"尖 − 尾"得到的仍是同一个 $\mathbf v$：箭头的身份是方向和长度，不是位置。

用青色小环把副本滑到别处（至少挪开 1.5），确认右边"尖 − 尾"的结果没有变。
把 $\mathbf v$ 拖成 $(-2,\,3)$。

核心洞见：位移与位置

位移 (displacement) 回答"从这里走到那里要走多少"，不需要原点，可以随意平移。位置 (position) 回答"在哪里"，必须先约定原点和坐标轴；它其实是"从原点出发的位移"。本节把向量主要当作位移来用。第 4 节会专门玩这个区别，它是后面所有坐标系内容（5.1 节）的伏笔。

3动手玩：向量实验室

现在把两个向量放在一起。拖动琥珀色的 $\cv{\mathbf a}$ 和紫色的 $\cw{\mathbf b}$ 的箭头尖（会吸附到 0.5 的格点上），再用右上角的开关选一种运算：

加法 $\mathbf a+\mathbf b$：先走完 $\mathbf a$，再从它的尖出发走 $\mathbf b$（首尾相接）。先走 $\mathbf b$ 再走 $\mathbf a$ 也到同一点，两条路围成一个平行四边形。
减法 $\mathbf a-\mathbf b$：从 $\mathbf b$ 的尖指向 $\mathbf a$ 的尖的那支箭头。
数乘 $s\,\mathbf a$：沿 $\mathbf a$ 所在的直线拉长或缩短；$s$ 为负时掉头。

FIG 2向量实验室

预设

加法档：虚线是把 $\mathbf b$ 接在 $\mathbf a$ 的尖上、再把 $\mathbf a$ 接在 $\mathbf b$ 的尖上，两条路到同一点。减法档：$\mathbf a-\mathbf b$ 的虚线副本被平移到原点，数字相同。数乘档（用下面的滑块调 $s$）：$s\,\mathbf a$ 永远在 $\mathbf a$ 所在的那条虚线上。

试一试

让 $\mathbf a+\mathbf b=\mathbf 0$（两条路走回原点）。$\mathbf b$ 该放在哪里？
让 $|\mathbf a+\mathbf b| = |\mathbf a|+|\mathbf b|$（看右边的读数）。$\mathbf a$ 和 $\mathbf b$ 必须满足什么关系？
切到 $\mathbf a-\mathbf b$ 档，让它恰好等于 $(1,\,2)$。注意 $\mathbf a$ 和 $\mathbf b$ 都可以动，答案不止一个。
切到 $s\,\mathbf a$ 档，让它与 $\mathbf a$ 方向相反、长度恰为 $\mathbf a$ 的一半（$s=-0.5$）。

小游戏：够到目标

机器人每一步只能走两种固定的位移：琥珀色的 $\cv{\mathbf d_1}$ 或青色的 $\cx{\mathbf d_2}$，可以前进，也可以后退。点按钮，把一段段位移首尾相接，让代表机器人的大圆点停在紫色的目标 $\cw{T}$ 上。淡青色的小点是所有"整数份 $\mathbf d_1$ 加整数份 $\mathbf d_2$"能到的位置，目标一定在其中。试试用最少的步数。

FIG 3够到目标

每个目标都有唯一的最短配方：走几次 $\mathbf d_1$、几次 $\mathbf d_2$ 是确定的（两个步向量方向不同，所以组合方式只有一种），先走哪一步则随你。用最少的步数命中，会得到"最短"的标记。

命中 3 个目标。

4第二个视角：点与向量不是一回事

前面有件事一直悄悄成立：原点在画面中央。可现实里原点是人选的：世界坐标系的原点可能在房间的一角，也可能在机器人底座上。同一个杯子，换一个原点，坐标就变了。杯子没动，动的是尺子。

于是有一条要紧的区分。点的坐标依赖原点；从点 $P$ 到点 $Q$ 的位移 $\mathbf d = Q-P$ 却不依赖，因为原点一挪，$P$ 和 $Q$ 的坐标同时减去同一个数，差不变。这给出四条规则：

点 − 点 = 向量（位移）；
点 + 向量 = 点（走了一段位移之后到哪里）；
向量 + 向量 = 向量（位移接位移）；
点 + 点 = 没有意义：结果会随原点而变。

下面的图里，杯子 $P$ 和盒子 $Q$ 放在桌上，拖动原点 $O$（青色）时它们原地不动；你也可以单独拖动 $P$、$Q$。网格是以 $O$ 为原点画的坐标纸，右侧显示 $P$、$Q$ 在这套坐标下的读数。

FIG 4挪动原点

在以 O 为原点的坐标系里

位移（与原点无关）

"点 + 点"（依赖原点）

虚线是 $P$、$Q$ 的位置向量（从 $O$ 出发的箭头）：挪动 $O$，它们跟着变。琥珀色的 $\mathbf d=Q-P$ 纹丝不动。红色圆环是把两个读数相加得到的"$P+Q$"，它会随 $O$ 乱跑。

把原点 $O$ 拖到杯子 $P$ 上，使 $P$ 读作 $(0,0)$。这时 $Q$ 的读数等于什么？
找一个原点，使"$P+Q$"读作 $(0,0)$。它在 $P$ 和 $Q$ 的什么位置？

核心洞见

机器人里的"点"（杯子位置、关节位置）和"向量"（位移、速度、力）是两类不同的东西，但它们都用数字来写，很容易混。凡是涉及位置，就要问一句："相对哪个原点、哪个坐标系？"这个问题在 5.1 节会变成整套机器人位姿理论的出发点。

5第三个视角：很多个分量

到现在为止，向量只有两三个分量，因为我们能把它画出来。可机器人常常要处理分量很多的向量。一个六轴机械臂的构型 (configuration)，也就是每个关节转了多少，是 6 个角度 $\mathbf q=(q_1,\dots,q_6)$：一个 $\R^6$ 中的向量。

六维的箭头画不出来，但向量的本质从来不是"箭头"，而是"能相加、能数乘的一串数"。加法、数乘、长度，全都逐分量地照做，和二维时一模一样。

下图用一个平面三连杆臂代替六轴臂，它的构型是 $\mathbf q\in\R^3$。左边是臂，右边是 $\mathbf q$ 的三个分量，画成三根柱子。拖动臂的关节或柱子的顶端，另一边会跟着变。再试试预设按钮，看看对 $\mathbf q$ 做加法和数乘，臂会怎么动。

FIG 5关节角向量 q ∈ ℝ³

机械臂

q 的三个分量

颜色对应：$\ci{q_1}$ 是红色的第一节，$\cj{q_2}$ 是绿色的第二节，$\ck{q_3}$ 是蓝色的第三节。每个关节角都是相对前一节量的。紫色虚线圆环是目标。预设里的 A 和 B 是两个固定构型。

让手（末端）碰到紫色目标环。
点 A + B：逐分量相加之后，对比读数里"手(A) + 手(B)"和"手(A + B)"。

另一种看法：关节空间里的加法 vs 手的位置

$\mathbf q$ 本身是向量，所以 $\mathbf A+\mathbf B$ 完全合法，它就是"逐个关节把两个角度加起来"。但手的位置不是 $\mathbf q$ 的线性函数：手(A + B) 一般不等于手(A) + 手(B)。从关节角到手的位置那条映射是弯的，把它在一点附近拉直成矩阵，就是雅可比（3.3 节）。另外，角度每 360° 绕一圈，严格地说构型空间是环面而不是平坦的 $\R^n$。这里先忽略，Lynch & Park 第 2 章有完整的讲法。

6再看数学

提示：把鼠标悬停在公式中带底色的符号上，FIG 2 或 FIG 6 里对应的箭头会被加粗。

记号

$\R^n$ 是所有"n 个实数排成一列"的集合：$\R^2$ 是平面，$\R^3$ 是空间。本课程里向量一律写成列向量：

$$ \mathbf v = \begin{bmatrix} v_1 \\ v_2 \\ \vdots \\ v_n \end{bmatrix} \in \R^n, \qquad \mathbf v\T = \begin{bmatrix} v_1 & v_2 & \cdots & v_n \end{bmatrix}. $$

$\mathbf v\T$（转置）是同一串数写成一行。为什么需要它，1.7 节会讲。

加法、减法与数乘

三种运算都是逐分量的：

$$ \begin{aligned} \hl{sum}{\mathbf a + \mathbf b} &= \begin{bmatrix} \cv{a_1} + \cw{b_1} \\ \cv{a_2} + \cw{b_2} \end{bmatrix}, & \hl{diff}{\mathbf a - \mathbf b} &= \begin{bmatrix} \cv{a_1} - \cw{b_1} \\ \cv{a_2} - \cw{b_2} \end{bmatrix}, & \hl{s}{s\,\mathbf a} &= \begin{bmatrix} s\,\cv{a_1} \\ s\,\cv{a_2} \end{bmatrix}. \end{aligned} $$

几何上：加法是"首尾相接"，数乘是"沿原方向缩放（$s<0$ 时掉头）"，减法是 $\mathbf a + (-1)\,\mathbf b$，也就是从 $\mathbf b$ 的尖指向 $\mathbf a$ 的尖。所有分量都是 0 的向量叫零向量 $\mathbf 0$。

运算法则

对任意向量 $\mathbf a,\mathbf b,\mathbf c$ 和数 $s,t$：

$$ \begin{array}{ll} \mathbf a+\mathbf b=\mathbf b+\mathbf a & \text{(commutative)}\\ (\mathbf a+\mathbf b)+\mathbf c=\mathbf a+(\mathbf b+\mathbf c) & \text{(associative)}\\ \mathbf a+\mathbf 0=\mathbf a,\quad \mathbf a+(-\mathbf a)=\mathbf 0 & \\ s(\mathbf a+\mathbf b)=s\mathbf a+s\mathbf b,\quad (s+t)\mathbf a=s\mathbf a+t\mathbf a & \text{(distributive)}\\ (st)\,\mathbf a=s(t\,\mathbf a),\quad 1\,\mathbf a=\mathbf a & \end{array} $$

每一条都可以逐分量验证，因为它们对普通的数成立。交换律的几何含义就是 FIG 2 里的平行四边形：两条路到同一点。结合律说的是：多段位移首尾相接，先把哪两段并起来都一样。这就是第 7 节里里程计可以"一步步累加"的原因。

长度（范数）

二维向量 $\mathbf v=(x,y)$ 的长度，是以 $x$ 和 $y$ 为两条直角边的直角三角形的斜边，由勾股定理：

$$ |\hl{norm}{\cv{\mathbf v}}| = \sqrt{\hl{nx}{\ci{x^2}} + \hl{ny}{\cj{y^2}}}. $$

FIG 6长度与单位向量

拖动琥珀色的 $\mathbf v$：接近整数格点或单位圆（青色虚线）时它会吸附。红、绿两条边是 $x$、$y$ 分量；青色的 $\hat{\mathbf u}$ 是和 $\mathbf v$ 同方向、长度为 1 的向量。注意"两条边的长度之和"总是不小于斜边。

找一个整数分量、长度恰为 5 的向量，而且不在坐标轴上。
让 $\mathbf v$ 成为单位向量，且不沿坐标轴方向。

三维时把勾股定理用两次：先在水平面里得到斜边 $d=\sqrt{x^2+y^2}$，再把 $d$ 和竖直分量 $z$ 当作两条直角边：$|\mathbf v|=\sqrt{d^2+z^2}$。例如 $\mathbf v=(2,3,6)$：$d^2=2^2+3^2=13$，$|\mathbf v|=\sqrt{13+36}=\sqrt{49}=7$。同理对任意维数，分量的平方和再开方：

$$ |\mathbf v| = \sqrt{v_1^2 + v_2^2 + \cdots + v_n^2}, \qquad |s\,\mathbf v| = |s|\,|\mathbf v|, \qquad |\mathbf a + \mathbf b| \le |\mathbf a| + |\mathbf b|. $$

最右边是三角不等式：走两段位移，总位移不会比两段长度之和更长。只有当 $\mathbf a$、$\mathbf b$ 同向（或其中一个为零）时才取等号。这正是 FIG 2 里第二个任务的答案。两点 $P$、$Q$ 之间的距离就是位移的长度 $|Q-P|$。

单位向量

把非零向量除以自己的长度，就得到只保留方向的单位向量 (unit vector)。反过来，任何向量都是"长度 × 方向"：

$$ \hl{unit}{\hat{\mathbf u}} = \frac{\cv{\mathbf v}}{|\cv{\mathbf v}|}, \qquad |\hat{\mathbf u}|=1, \qquad \cv{\mathbf v} = |\cv{\mathbf v}|\,\hat{\mathbf u}. $$

零向量没有方向，所以 $\hat{\mathbf u}$ 对 $\mathbf v=\mathbf 0$ 无定义，代码里要先判断长度不为 0。

点与向量的运算，用符号写一遍

第 4 节的四条规则：$Q-P=\mathbf d$（向量），$P+\mathbf d=Q$（点），$\mathbf d_1+\mathbf d_2$（向量），$P+Q$（无意义）。有一种"点加点"是允许的：权重加起来等于 1 的组合，例如中点 $\tfrac12(P+Q)=P+\tfrac12(Q-P)$。它其实是"从 $P$ 出发走一半位移"，所以不依赖原点。

7机器人中的它：里程计与航位推算

一个移动机器人不知道自己在哪，但每一步都能估计"这一步走了多少"（轮子编码器的计数、IMU 的积分）。每一步是一个位移向量 $\mathbf s_i$。把它们首尾相接，总位移就是向量之和；机器人现在的位置是"起点 + 总位移"，正是第 4 节的"点 + 向量 = 点"：

$$ \mathbf p_N = \mathbf p_0 + \sum_{i=1}^{N} \cv{\mathbf s_i}. $$

这种只靠累加位移来估计位置的方法叫航位推算 (dead reckoning)。琥珀色是真实的路径，紫色虚线是带噪声的估计：每一步的估计 = 真值 + 一点随机误差。拖动任意一个节点，改变那一步的位移（后面的节点会整体平移）；用滑块加噪声，看终点的误差怎么积累。

FIG 7步子连成路径

不带颜色的虚线箭头是总位移 $\sum\mathbf s_i$，它连接起点和真实终点，与中间怎么走无关。红色箭头是估计终点相对真实终点的误差，它等于各步噪声之和。

拖动节点，让路径回到起点（总位移为零）。路径的总长度还能很长吗？
加大噪声，让终点误差超过 1 m。

真实系统里

轮式里程计给出的步长是在机器人自己的坐标系里量的（"向前 0.3 米"），累加之前要先用旋转矩阵换到世界坐标系（1.10 节、5.1 节），本图为了专注向量加法，假设步长已经在世界系里。误差会随步数越积越大，所以实际系统总要用 GPS、激光匹配或卡尔曼滤波（4.8 节）不断校正。

同一个想法还有一个更小的例子：速度也是向量，可以相加。一条船相对水以 3 m/s 向北开，水流以 1 m/s 向东，那么船相对地面的速度是 $(0,3)+(1,0)=(1,3)$ m/s，速率 $\sqrt{10}\approx3.16$ m/s，航向偏离正北向东约 18.4°。无人机顶着侧风飞、传送带上的机械臂抓取，都是同样的加法。

8常见误区

误区 1：把两个位置相加

"杯子在 (2, 1)，盒子在 (4, 3)，两者相加得 (6, 4)"，这个数没有物理含义：换一个原点它就变了（FIG 4）。要加的是位移，要减的才是两个位置。想求"两者的中间"，用 $\tfrac12(P+Q)$，因为它的权重之和是 1。

误区 2：把不同单位的量放进同一个向量再算长度

机器人位姿常写成 $(x,y,\theta)$，前两项是米，第三项是弧度。$\sqrt{x^2+y^2+\theta^2}$ 这个"长度"没有意义：把 $\theta$ 改用度，数值就完全不同（见小练习第 5 题）。要比较两个位姿的"远近"，必须先决定权重，例如乘上一个特征长度，把角度换成"弧长"。

误区 3：长度等于分量之和

误区 4：代码里的"行"与"列"

本课程里向量是列向量，但 NumPy 的一维数组 v = np.array([1.0, 2.0, 3.0]) 的形状是 (3,)，既不是行也不是列：对它做 v.T 什么也不会发生。a * b 是逐元素相乘，不是点积（点积用 a @ b，1.7 节）；长度用 np.linalg.norm(v)，不是 sum(v)。需要真正的列向量时写 v.reshape(3, 1)。

9小练习

1. 机器人在坐标系 A 里测得杯子 $P=(3,1)$、盒子 $Q=(7,4)$。现在把坐标系的原点向右平移 1（物体都没动）。下面哪个量不会改变？

新读数是 $P=(2,1)$、$Q=(6,4)$，每个坐标的 x 都减了 1。$Q-P$ 仍是 $(4,3)$；$P+Q$ 从 $(10,5)$ 变成 $(8,5)$；$|Q|$ 从 $\sqrt{65}\approx8.06$ 变成 $\sqrt{52}\approx7.21$。位移不依赖原点。

2. 设 $\mathbf a=(2,-1)$，$\mathbf b=(-3,4)$。$2\mathbf a-\mathbf b$ 等于？

$2\mathbf a=(4,-2)$，再减去 $\mathbf b$：$(4+3,\,-2-4)=(7,-6)$。其他选项分别是 $2\mathbf a+\mathbf b=(1,2)$、$2\mathbf b-\mathbf a=(-8,9)$、$\mathbf a+\mathbf b=(-1,3)$，都是符号或系数弄错的结果。

3. $\mathbf v=(3,-4)$ 方向上的单位向量是？

$|\mathbf v|=\sqrt{9+16}=5$，所以 $\hat{\mathbf u}=(3/5,-4/5)=(0.6,-0.8)$。$(3/7,-4/7)$ 是除以了分量之和（长度 $5/7\ne1$）；$(3/25,-4/25)$ 是除以了长度的平方（长度 $1/5$）；$(0.8,-0.6)$ 长度虽是 1，方向却不对（它更偏向 x 轴）。

4. 机器人依次向东走 3 m、向北走 4 m、向西走 3 m。它的总位移是？

$(3,0)+(0,4)+(-3,0)=(0,4)$，长度 4 m。路程（各步长度之和）是 $3+4+3=10$ m，位移长度只有 4 m：$|\sum\mathbf s_i|\le\sum|\mathbf s_i|$。东西分量抵消了，但南北分量没有。

5. 位姿 $(x,y,\theta)=(3\text{ m},\,4\text{ m},\,0.5\text{ rad})$。有人算它的"长度" $\sqrt{3^2+4^2+0.5^2}\approx5.02$。这个做法如何？

公式本身没错，问题在于米和弧度不是同一种量，加起来的结果取决于你选了什么单位：用度时 $\sqrt{9+16+28.65^2}\approx29.08$。"弧度"只是碰巧让数值看起来合理。要衡量位姿之间的差距，需要显式地加权，例如 $\sqrt{x^2+y^2+(\ell\theta)^2}$，其中 $\ell$ 是一个以米为单位的特征长度（比如机器人半径）。

10延伸阅读

3Blue1Brown, Essence of Linear Algebra — 第 1 章"向量究竟是什么？"（箭头、数字、抽象三种看法）
G. Strang, Introduction to Linear Algebra, Ch. 1 — 向量入门：线性组合与长度
K. Lynch & F. Park, Modern Robotics, Ch. 2 — 构型空间：为什么机器人的状态是 $\R^n$ 中的一个点（以及它什么时候不是平坦的）
J. Ström et al., Immersive Linear Algebra, Ch. 2 — 带交互插图的向量一章