第一章 觉醒的“漫游者”：机器人如何理解所处的世界？

凌晨三点的深圳南山区机器人测试场，日光灯管发出轻微的嗡鸣，金属冷却液的味道混着纸箱的纤维味飘在空气里。这是我作为机器人算法工程师的第三年，已经习惯了和这些铁家伙一起熬夜——但那天晚上，当我看到四轮差速的自主移动机器人“小步”在堆满障碍物的通道里稳稳绕开纸箱，精准停在通道尽头的标记点上时，还是突然生出一种奇怪的感动：这个没有生命的铁盒子，好像真的“看懂”了自己身处的世界。

它顶部的激光雷达每秒旋转10圈，绿色的点云在它的“眼睛”里扫过障碍物的轮廓；底部的轮子编码器精确记录着每一步的位移，IMU感知着自身的倾斜和转向；片刻后，轮子微微调整角度，沿着一条平滑的弧线绕过挡路的纸箱，最终停靠的误差不超过2厘米。这个“认路—思考—行动”的过程，就是机器人学过去六十年一直在打磨的核心能力，也是这本书要跟着机器人“漫游”的全程：记录它怎么理解所处的世界，怎么规划行动的路线，怎么精准执行每一个动作，背后的数学本质和工程实践。

<!-- 标签：机器人发展史 -->

一、从“盲行者”到“漫游者”：机器人的六十年进化

如果把机器人比作漫游者，它的进化史本质上就是“感官”和“大脑”不断升级的历史。最早的机器人连“看”的能力都没有，只能按固定轨迹走，是彻头彻尾的“盲行者”；后来有了传感器，能感知障碍，变成了能避开固定障碍的“近视眼”；再后来有了建图和定位能力，能在陌生环境里自己认路，才真正成了能自主漫游的“探索者”；而今天的人形机器人，已经能理解人类的语言、预判人类的动作，成了能和人协同工作的“伙伴”。

这一切的起点，是1961年通用汽车工厂里那台叫Unimate的工业机器人。它长着液压驱动的机械臂，能精准地把传送带上的热压铸件抓起来放到筐里，动作比最熟练的工人还稳，而且永远不会累。它的核心逻辑是“示教再现”：工程师手动移动机械臂到指定位置，记录下每个关节的角度，机器人就能重复这个动作一遍又一遍。但当时的Unimate没有“眼睛”，看不到周围的世界，如果有人在它的工作路径上站着，它不会停，只会把人撞开——工人们最初叫它“钢铁

工人们最初叫它“钢铁怪物”——毕竟在1961年的新泽西州特伦顿通用汽车工厂里，这个身高1.8米、重达1.5吨的液压机械臂没有任何感知能力，它的工作路径是工程师提前用示教器手动记录的，如果有人误入它的工作区域，它不会停下，只会按照预设轨迹继续运动，轻则把人撞倒，重则造成严重的工伤。第一年上岗的时候，Unimate就出了3起小事故，直到工厂给它加装了简单的光电限位开关，才避免了更严重的后果。

但哪怕有这样明显的缺陷，Unimate依然掀开了机器人时代的序幕。它代替工人完成了热压铸件的搬运工作，动作精度达到了±0.1毫米，相当于一根头发丝的五分之一，而且可以24小时不间断工作，直接把生产效率提升了30%。它的核心逻辑是“示教再现”：工程师手动移动机械臂到目标位置，记录下每个关节的角度和运动轨迹，机器人就能无限重复这个动作。这种逻辑在今天依然被用在很多工业场景里，比如焊接、喷涂、装配等标准化程度高的工序，但它的本质是“没有感官的盲行者”——它不知道自己在哪，不知道周围有什么，只是机械地执行预设的程序。

Unimate的局限性很快暴露出来：只要工作场景有一点变化，比如传送带的位置偏移了1厘米，或者铸件的摆放角度变了，它就会抓空或者把零件碰坏。要解决这个问题，机器人首先需要“眼睛”，也就是传感器。

1980年代，随着计算机视觉和传感器技术的发展，第二代机器人开始普及。这类机器人最典型的代表是SCARA（选择性顺应性装配机器手臂）机器人，它自带视觉引导系统，能通过摄像头识别零件的轮廓和位置，自动调整抓取姿态，精度比Unimate又提升了10倍。但这类机器人依然被固定在某个工作台上，活动范围有限，还是没办法自主移动。

真正让机器人从“固定在工作台上的操作者”变成“能自主漫游的探索者”的，是1969年斯坦福研究院发布的Shakey机器人。这个身高1.2米、带着四个轮子的铁盒子，是人类历史上第一个集成了感知、规划、控制三大模块的移动机器人：它的顶部装着一台摄像头，前面装着一台早期激光测距仪（LRP），能测量前方物体的距离，底部有轮子编码器和惯性测量单元（IMU），能记录自身的运动状态；它的“大脑”是一台DEC PDP-11计算机，体积比家用冰箱还大，算力还不如现在的一只智能手表。

【📌 电子版侧边栏：Shakey的冷知识】 Shakey的名字来源于它摇晃的行走姿态——因为当时的轮子控制算法还不成熟，它走直线的时候会左右摇摆，像个喝醉的人。它的研发团队花了10年时间，投入了超过2000万美元（相当于现在的1.5亿美元），最终只实现了非常基础的能力：在简单的办公室里自己开门、绕开固定的障碍物、走到指定地点拿起指定的物体，而且走10米需要20分钟，中间还经常卡在门槛上。但它的意义不在于能力有多强，而在于它第一次验证了“机器人自主漫游”的可行性，提出了SLAM（同步定位与建图）、路径规划、行为树等现在机器人领域的核心概念，只是当时的计算机算力太差，这些概念直到30年后才真正落地。

Shakey的局限性非常明显：它的激光测距仪每秒只能转1圈，测距精度只有±5厘米，建一张100平米的办公室地图需要几个小时；它的路径规划算法是基于符号逻辑的，遇到稍微复杂一点的环境就会陷入死循环；它的轮子控制是开环的，稍微有点坡度就会偏移路线。但Shakey就像第一个学会走路的婴儿，虽然摇摇晃晃，却指明了机器人未来的方向。

接下来的30年，机器人领域一直在解决Shakey留下的三个核心问题：感知的精度和速度、规划的效率和鲁棒性、控制的精准度。直到2010年前后，三个关键技术同时成熟，才让自主移动机器人（AMR）真正从实验室走向了工业场景： 第一个是激光雷达的成本大幅下降。2005年的时候，一台64线的激光雷达售价超过3万美元，只有科研院所买得起；到2020年，国产的16线激光雷达售价已经降到了1000元人民币以内，普通的小公司也能批量采购。 第二个是SLAM算法的成熟。2007年，加州大学伯克利分校的Spark实验室提出了基于图优化的激光SLAM算法，把建图和定位的精度提升到了厘米级，速度提升到了每秒10帧以上，终于能满足实时漫游的需求。 第三个是机载算力的提升。2010年之后，嵌入式ARM芯片的算力每两年翻一倍，现在一块几百元的RK3588开发板，算力已经相当于10年前的笔记本电脑，能实时运行SLAM、路径规划、目标检测等复杂算法。

这三个条件的成熟，让AMR在最近10年迎来了爆发。根据国际机器人联合会（IFR）2024年发布的报告，2023年全球AMR的出货量达到了42.8万台，同比增长28%，其中仓储物流场景的占比超过70%。最典型的应用就是亚马逊仓库的Kiva机器人（2012年被亚马逊收购后改名为Amazon Robotics），截至2024年，亚马逊仓库里已经部署了超过70万台Kiva机器人，它们顶部的激光雷达每秒扫描10万点，实时构建仓库的地图，通过SLAM算法定位自己的位置，精度达到±1厘米；全局路径规划器会规划从货架到拣货站的最短路径，局部规划器会实时避让其他机器人、工人和临时堆放的货物，控制模块会调整四个轮子的转速和转向，让机器人平稳运行，速度最高达到2米/秒，相当于成年人快走的速度。整个“感知-规划-控制”的闭环延迟不到100毫秒，比人类反应速度快3倍。

如果说工业机器人和AMR是机器人的“四肢”，那么人形机器人就是机器人进化的“终极形态”——它不仅能漫游，还能和人类一样用双手操作物体，用双腿上下楼梯，理解人类的语言和动作，成为人类的协同伙伴。2023年以来，特斯拉的Optimus、波士顿动力的Atlas、小米的CyberOne、优必选的Walker等产品陆续发布，虽然目前还处于试验阶段，但已经能实现自主行走、避障、搬运物体、递送物品等基础能力，背后依然是感知、规划、控制三大模块的支撑：顶部的深度相机和激光雷达感知周围的环境和人类的位置，语义理解模块识别人类的指令和物体的类别，全身规划模块协调双腿和双臂的动作，控制模块保证每一步都稳，不会摔倒。

从1961年的Unimate到2024年的人形机器人，机器人的进化史本质上就是“感知、规划、控制”三大模块不断升级的历史：感知从无到有，从单传感器到多传感器融合，从只能感知距离到能理解语义；规划从固定轨迹到全局路径，从静态环境到动态环境，从单机器人到多机器人协同；控制从开环到闭环，从单关节到全身协调，从位置控制到力控。这三大模块，就是机器人理解世界、自主行动的核心，也是这本书要深入讲解的全部内容。

<!-- 标签：机器人核心模块 -->

二、机器人的“大脑、眼睛和四肢”：三大核心模块的关系

我们可以把机器人想象成一个正在陌生城市漫游的游客：它首先需要用眼睛（摄像头、激光雷达等传感器）看周围的建筑、路牌、行人，用耳朵（麦克风、超声波传感器）听周围的声音，用身体（IMU、轮子编码器）感知自己的移动速度和姿态，这就是感知层——它是机器人认知世界的基础，所有后续决策的前提都是感知数据的准确性。

如果感知数据不准，再强大的规划算法也会失效。比如激光雷达的测距误差有10厘米，机器人规划路径的时候就会离障碍物太近，甚至撞上去；如果摄像头的光线太暗，目标检测算法识别不到行人，机器人就可能撞到突然走出来的人。我在做测试的时候遇到过很多这样的案例：机器人在强光下运行的时候，因为摄像头的曝光过度，点云里出现了很多噪点，SLAM算法直接把噪点当成了障碍物，规划出来的路径绕了一大圈，最后还卡在了角落里。后来我们加了光照自适应的算法，调整摄像头的曝光参数，才解决了这个问题。这个案例就说明，感知层的误差会直接传导到规划和控制层，必须作为最基础的一环打磨。

当机器人感知清楚了周围的环境和自己的位置之后，就需要决定“要去哪里、怎么走”，这就是规划层——它是机器人的“大脑”，负责把高层的任务目标转换成低层的可执行动作。规划层通常分为三层：最上层是全局路径规划，根据地图和目标点规划出一条从当前位置到目标点的最优路径，比如从仓库的A区到B区的最短路径，通常会用到A*、Dijkstra等图搜索算法；中间层是局部避障规划，负责避开路径上突然出现的动态障碍物，比如突然走过来的工人、其他机器人，通常会用到动态窗口法（DWA）、时间弹性带（TEB）等算法；最下层是运动轨迹规划，负责生成机器人每一步的具体动作，比如什么时候加速、什么时候转弯、转弯半径多大，通常会用到样条插值、模型预测控制（MPC）等算法。

规划层的核心矛盾是“最优性”和“实时性”的平衡：如果你要规划全局最优的路径，可能需要花几秒钟甚至几分钟的时间，但机器人在移动的时候，障碍物是动态变化的，等你规划完了，障碍物可能已经换了位置；如果你要实时规划，就只能牺牲一部分最优性，比如绕一点远路，但能保证不会撞到障碍物。我在做仓储机器人的路径规划的时候，曾经把全局路径规划的搜索范围从10米扩大到了50米，结果规划时间从10毫秒涨到了200毫秒，机器人在密集的货架中间移动的时候经常因为规划不及时撞到货架，最后我们不得不把搜索范围缩小到20米，虽然路径不是全局最优，但规划时间稳定在30毫秒以内，完全满足实时性的要求。这个案例就说明，工程实践里没有绝对的最优，只有最适合场景的权衡。

当规划层生成了一条可行的路径或者轨迹之后，就需要控制层来精准执行——它是机器人的“四肢和神经”，负责把规划出来的动作转换成具体的控制指令，比如给轮子电机发送多大的电压，让机械臂的关节转到指定的角度，保证机器人严格按照规划的轨迹运动，不会出现偏差。控制层最常用的是PID控制，它的原理非常简单：根据规划的目标值和实际的传感器反馈值之间的误差，计算出控制量，不断修正误差，让实际值无限接近目标值。比如机器人规划了向前走1米，轮子编码器反馈已经走了0.9米，PID控制器就会加大轮子的转速，直到走了1米为止。

控制层的核心挑战是“抗干扰”和“稳定性”：机器人运动的时候会遇到各种干扰，比如地面不平、风力、负载变化，都会让实际轨迹偏离规划的轨迹，控制器需要快速修正这些误差，同时不能出现振荡或者失控。我遇到过最极端的情况是：冬天仓库的地面结了薄冰，轮子的摩擦力突然下降，机器人按照规划的速度转弯的时候直接打滑，偏离了路径1米多，普通的PID控制器根本修正不过来，后来我们加了滑模控制算法，根据轮子的打滑情况动态调整控制量，才解决了这个问题。

感知、规划、控制三个模块不是孤立的，而是形成了一个闭环：感知层采集的环境和自身状态数据传给规划层，规划层生成的动作指令传给控制层，控制层执行之后的实际状态又通过感知层反馈回来，不断修正误差，让机器人能稳定漫游。我们可以用下面的流程图来表示这个闭环：

```mermaid graph LR A[感知层：传感器采集环境+自身状态数据<br>（激光雷达、摄像头、IMU、编码器等）] --> B[认知层：SLAM建图、目标识别、语义理解<br>（点云匹配、特征提取、目标检测等）] B --> C[规划层：全局路径规划、局部避障、运动轨迹生成<br>（A*、DWA、MPC等算法）] C --> D[控制层：轮速/关节控制、力控、姿态稳定<br>（PID、滑模控制、全身控制等）] D --> A ```

这个闭环是机器人学的核心框架，这本书的所有内容都是围绕这个闭环展开的：第2到5章讲感知层的数学本质和工程实践，第6到10章讲规划层的算法和优化，第11到15章讲控制层的理论和应用。搞懂了这个闭环，你就搞懂了机器人漫游的全部逻辑。

<!-- 标签：本书定位 -->

三、本书的定位：打通理论和实践的鸿沟

市面上关于机器人的书大概分为三类：一类是科普向的，比如《机器人简史》《人工智能简史》，适合普通读者了解机器人的发展历程，但不会深入讲技术细节；一类是学术教材向的，比如《概率机器人》《机器人学导论》，讲了很多数学原理，但缺乏工程实践的指导，很多读者看完之后还是不知道怎么动手写代码；还有一类是工具书向的，比如ROS官方教程、开源SLAM包的文档，适合查API，但不会讲底层的原理。

这本书的定位是给有一定基础的中级读者的技术进阶书：我们不会讲太基础的科普内容，也不会堆砌复杂的数学公式，而是把每个算法的数学本质和工程实践结合起来，告诉你“这个算法为什么有用、怎么用、实际用的时候会遇到什么问题、怎么解决”。比如讲SLAM的时候，我们不会只给你讲图优化的公式，还会告诉你实际做激光SLAM的时候，怎么调整回环检测的参数，怎么解决动态障碍物导致的建图漂移问题，怎么优化代码让SLAM在嵌入式设备上实时运行。

我们的目标读者是：

• 机器人、自动化、计算机相关专业的大学生，已经学过线性代数、概率论，会Python编程，做过一两个小的机器人项目，想深入搞懂机器人算法的底层原理，而不是只会调开源包；
• 刚入职的机器人算法工程师，已经会用ROS和开源算法做简单的开发，但遇到问题不知道怎么排查，想提升自己的技术深度，能独立解决复杂的工程问题；
• 机器人领域的爱好者，已经玩过一些开源机器人平台，比如Arduino、树莓派、ROS机器人，想系统学习机器人的核心技术，做出能真正自主漫游的机器人。

如果你符合上面的任意一条，这本书就是为你写的。我们会尽量用通俗的语言讲清楚复杂的原理，用实际的案例和代码示例帮你把理论落地，每章最后还会留一些练习题，帮你巩固所学的内容。

<!-- 标签：前置知识 -->

四、你需要具备的前置知识

机器人学是一个交叉学科，涉及到数学、计算机、自动化、机械等多个领域，要搞懂这本书的内容，你需要具备以下几方面的基础知识，如果你还不具备，建议先花1-2周时间补一下，不然看后面章节的时候会很吃力。

4.1 线性代数基础

线性代数是机器人学的数学基础，几乎所有的感知、规划、控制算法都会用到。你需要掌握以下内容： 1. 矩阵的基本运算：矩阵乘法、求逆、转置、行列式，这些是坐标变换、状态估计的基础。比如机器人从世界坐标系到自身坐标系的变换，就是用旋转矩阵和平移向量组合成的齐次变换矩阵表示的： $$T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}$$ 其中$R$是3×3的旋转矩阵，$t$是3×1的平移向量，$0$是1×3的零向量，1是常数项。这个矩阵可以把世界坐标系下的任意点$(x,y,z)$转换成机器人自身坐标系下的坐标$(x',y',z')$。 2. 特征值与特征向量：用于主成分分析（PCA）、点云配准等场景。比如激光雷达的点云降维，就是用PCA找到点云的主方向，把3维点云压缩到2维平面，提升运算速度。 3. 奇异值分解（SVD）：用于位姿估计、点云配准、最小二乘求解等场景。比如两个激光雷达的点云匹配的时候，就是用SVD分解计算两个点云之间的最优旋转和平移矩阵，让两个点云的误差最小。 4. 最小二乘：用于传感器数据融合、位姿优化等场景。比如机器人融合激光雷达和视觉的定位结果的时候，就是用加权最小二乘，把两个传感器的观测结果融合成最优的位姿估计。

如果你对上面的内容还不熟悉，建议先看《线性代数及其应用》（Gilbert Strang著）的前5章，重点看矩阵运算、特征值、SVD、最小二乘这几个部分，花3-5天就能把基础补上。

4.2 概率论基础

机器人感知的核心是处理不确定性：传感器的测量有噪声，环境是动态变化的，机器人的运动也有误差，所以需要用概率论的方法来建模和估计。你需要掌握以下内容： 1. 贝叶斯公式：概率论的核心，是机器人状态估计的基础。比如机器人的定位问题，就是根据传感器的观测数据，不断更新自己位置的概率分布，这个过程就是基于贝叶斯公式的： $$P(x|z) = \frac{P(z|x)P(x)}{P(z)}$$ 其中$P(x|z)$是机器人位置为$x$的概率（后验概率），$P(z|x)$是在位置$x$观测到数据$z$的概率（似然），$P(x)$是机器人位置的先验概率，$P(z)$是观测数据的归一化常数。 2. 高斯分布：最常用的概率分布，用于建模传感器的噪声。比如激光雷达的测距误差通常服从均值为0，方差为$\sigma^2$的高斯分布： $$d_{measured} = d_{true} + \epsilon, \quad \epsilon \sim \mathcal{N}(0, \sigma^2)$$ 其中$d_{true}$是真实距离，$d_{measured}$是测量值，$\epsilon$是测量噪声。我们在融合多传感器数据的时候，会给误差小（方差小）的传感器更高的权重，就是这个原理。 3. 马尔可夫链与马尔可夫假设：机器人运动的状态转移通常满足马尔可夫假设：当前状态只和上一时刻的状态有关，和更早的状态无关。这个假设大大简化了状态估计的计算，是卡尔曼滤波、粒子滤波等算法的核心前提。 4. **最大似然

接上之前中断的概率论内容，“最大似然”四个字刚写到一半，