第1章：坐标与向量基础

各位同学好，我是你们的避障算法讲师。今天咱们从最基础的东西聊起——坐标与向量。

你可能会想：「这玩意儿我高中就学过，还用讲？」

嗯，说实话，我当年也是这么想的。直到我在项目中踩了坑，才发现基础不牢，地动山摇。有一次做机器人路径规划，因为坐标系搞混了，机器人直接往墙上撞……那场面，别提多尴尬了。

所以，咱们老老实实把基础打牢。后面30章的内容，全得靠这些基础撑着。

1.1 二维与三维坐标系

先说坐标系。避障算法里，我们最常用的是笛卡尔坐标系。

二维坐标系很简单：一个x轴，一个y轴。原点在(0,0)。机器人在地面上移动，通常就用二维坐标表示位置。

三维坐标系呢？多了一个z轴。无人机、机械臂这些，就得用三维坐标了。

这里有个坑——坐标系的方向。不同的传感器、不同的库，坐标系定义可能不一样。比如ROS里用的是右手系，而某些摄像头用的是左手系。

所以，拿到数据第一件事：搞清楚坐标系定义。别偷懒。

1.2 向量加减法

向量是什么？说白了，就是带方向的箭头。

在避障里，机器人的速度、加速度、位置偏移，全都可以用向量表示。

向量加法：两个向量首尾相接，结果就是合向量。

举个例子：机器人先向东走3米，再向北走4米。位移向量分别是(3,0)和(0,4)。加起来是(3,4)。合位移是5米，方向东北偏东。

向量减法：A - B，就是从B指向A的向量。

这个在避障里特别有用。你想让机器人从当前位置A走到目标点B，方向向量就是B - A。

// 二维向量加减法示例
struct Vector2D {
    float x, y;
};

Vector2D add(Vector2D a, Vector2D b) {
    return {a.x + b.x, a.y + b.y};
}

Vector2D subtract(Vector2D a, Vector2D b) {
    return {a.x - b.x, a.y - b.y};
}

// 使用示例
Vector2D robotPos = {2.0, 3.0};
Vector2D targetPos = {8.0, 7.0};
Vector2D direction = subtract(targetPos, robotPos);
// direction = {6.0, 4.0}

1.3 点积与叉积

这两个东西，很多同学觉得抽象。我换个角度讲。

点积：判断方向关系

点积的公式：A · B = |A| * |B| * cos(θ)

说白了，点积衡量的是两个向量在同一个方向上的投影大小。

• 点积 > 0：方向大致相同（夹角小于90°）
• 点积 = 0：垂直
• 点积 < 0：方向相反（夹角大于90°）

在避障里，点积用来判断障碍物在机器人前方还是后方。比如，机器人的前进方向向量和障碍物方向向量做点积，正数说明障碍物在前面。

float dotProduct(Vector2D a, Vector2D b) {
    return a.x * b.x + a.y * b.y;
}

// 判断障碍物是否在机器人前方
Vector2D robotForward = {1.0, 0.0};  // 朝x轴正方向
Vector2D obstacleDir = {3.0, 2.0};   // 障碍物方向
float dot = dotProduct(robotForward, obstacleDir);
if (dot > 0) {
    // 障碍物在前方，需要避障
}

叉积：判断左右关系

叉积的结果是一个垂直于两个向量的新向量。在二维里，我们通常只看叉积的z分量。

公式：A × B = |A| * |B| * sin(θ)

• 叉积 > 0：B在A的左侧
• 叉积 = 0：共线
• 叉积 < 0：B在A的右侧

这个在路径规划里太常用了。判断障碍物在机器人的左边还是右边，决定往哪边躲。

float crossProduct2D(Vector2D a, Vector2D b) {
    return a.x * b.y - a.y * b.x;
}

// 判断障碍物在机器人左侧还是右侧
Vector2D robotForward = {1.0, 0.0};
Vector2D obstacleDir = {2.0, 1.0};
float cross = crossProduct2D(robotForward, obstacleDir);
if (cross > 0) {
    // 障碍物在左侧，向右躲
} else {
    // 障碍物在右侧，向左躲
}

1.4 欧氏距离与曼哈顿距离

距离度量，避障算法里绕不开的话题。

欧氏距离：直线距离。就是我们平时用尺子量的那个距离。

公式：d = sqrt((x2-x1)² + (y2-y1)²)

曼哈顿距离：只能横着走、竖着走的距离。像曼哈顿的街道，不能穿楼。

公式：d = |x2-x1| + |y2-y1|

距离类型	适用场景	计算量	特点
欧氏距离	自由移动的机器人、无人机	较大（有开方）	最真实，但计算慢
曼哈顿距离	网格地图、四方向移动	小（只有加减）	计算快，但不够精确

你想想看，在A*算法里，用曼哈顿距离做启发式函数，比欧氏距离快得多。但如果你做的是无人机避障，用曼哈顿距离就不合适了——无人机能斜着飞啊。

1.5 本章知识体系

下面这张图，把本章的核心逻辑串起来了。你仔细看看，后面30章的内容，全是从这里延伸出去的。

这张图你看懂了吗？从上到下，就是避障算法的完整链路。坐标系是基础，向量运算是工具，距离度量是判断依据。三者结合，才能做出正确的避障决策。

好了，第一章就到这里。内容不多，但都是硬通货。你把这些基础吃透了，后面学起来会轻松很多。

有什么问题，欢迎交流。咱们下章见。

---

公众号:蓝海资料掘金营，微信deep3321