========================
== Seeking Complexity ==
========================

E2E Self-Driving：3000行的端到端导航tinynav+tinybev

PUBLIC

人形机器人最近吸引了非常多的目光。

在 具身智能 的热潮下，可能首先需要做的，就是分清本体控制能力和智能能力。

它们不仅定义不同，技术路线也已经分道扬镳。

我更关注智能，而其中，相比于手上的能力，我也更关注腿上的能力，也就是导航的能力。

我计划动手写一套导航软件，可以：

  1. 多本体：四足狗，人形，小车
  2. 代码精简：选择这个领域的一些最佳实践，并且适合作为各种想法的试验田
  3. 容易上手：尽量少地需要调参，至少针对特定的本体和计算平台，开箱即用。

场景

在开始之前，我介绍几个场景，这样在理解下面的技术选择的时候，脑中有一个画面，可能更便于进一步讨论。

  1. 题图中的马拉松，基本是给定地图的固定路线巡线，但是希望带有局部避障能力，工厂机器狗巡检类似。
  2. 无人机的APAS功能，用户推杆大概告诉机器人的方位，然后机器人选择平滑安全的轨迹去执行，没有特定的终点。
  3. POI控制。在地图中打下若干个POI后，接收到达某点的指令去执行。比如扫地机回仓充电，或者酒店机器人送快递。

平台配置和定位

先从定位说起，选择的传感器配置是双目+IMU，第一等支持的模组就是：

可能对比几个其他的方法，更好理解这个选择：

  1. 为什么不用Lidar 确实要承认，现在似乎Lidar的定位非常稳定。从找简历的经历来看，能做Lidar定位的人至少是3倍多于能做Camera定位的人。

    但是，我依旧选择了双目为主。

    一个原因是，我觉得我能搞定，我不觉得相机在什么场景下会在理论上限上低于Lidar。

    第二个原因是，我还是希望这一套算法栈是用在2C产品上的，那么Lidar依旧是有明显成本劣势。

    第三个原因是，我希望能用算力换来性能。

  2. 为什么不用轮子，或者关节数据，或者GPS做紧耦合 应该是可以提高性能的。但是从过去的经验来看，每多一个传感器，都要多很多段故事。

    或者说如果我希望这套算法栈能被人用起来，传感器上的“紧密”程度越低越好。

    这里的权衡就是到双目模组。如果更低一步，变成单目+imu，感觉又太难了。

实现技巧

VIO本身并不复杂：最小化投影误差+最小化IMU积分误差即可。用的优化方法也是非常经典的高斯牛顿。

但是我上学时候写的VINS有一万五千行C++代码。

对比之下，tinynav的感知模块，带上双目，也只有639行。

VIO比较复杂的地方在于：

  1. 初始化比较难，pose依赖point cloud，point cloud依赖pose。不仅难实现，还很容易影响性能。
  2. bookkeeping比较复杂，给图查点，给点查图。

与之对应，简化实现的方法也主要有两个：

  1. 有双目之后，初始化变得很简单。也会更加稳定。
  2. Disjoint Set + memorized pure function，基本实现了等价的代码变换： 最终伪代码可以写成
x = argmin sum(cost_function(track) for track in disjointset())

轨迹算法

非常high-level地介绍轨迹算法，可以把它看作一个搜索问题。对比上面提到的定位更像一个优化问题。

顺便插入一句，这里的说法非常粗糙，如果说不关心问题的连续性，那么也可以说大家都是优化问题。

以搜索的视角，轨迹算法是在评价一系列选择之间的孰优孰劣。

这个评价，可以用判别的思路来做：给定若干条轨迹，对每一个进行打分，选择分数最高的。

也可以用生成式的方法来做：根据条件或者先验，随机生成若干轨迹，或者干脆选择概率最大的轨迹。

tinynav在这里选择的是判别式的方法，生成式的方法放在tinybev中用flow matching conditioned on BEV来做。另外再做介绍。

候选轨迹的表示，最直接的定义是控制信号，这里的选择是线速度和角速度，共3个自由度。

而轨迹的平滑分数中，最重要的是碰撞分数，由轨迹和双目深度raycasting得到的地图做碰撞计算得到。

模型推理

第一等支持的计算平台是Nvidia Jetson Orin Nano。所以对应的模型优化也变得标准化了。

在小电脑和大电脑中，提到过对于计算的分析，可以归结到roofline model。

这里遗留一个todo分析一下这几个模型的特征。

对于一个跑在edge的网络，其实也是一个有限的搜索问题，特别是给定输入尺寸之后。

有这么几个重要的工作：

  1. 等价的计算图变化，或者说最基础的消除掉对于output来说没有贡献的节点。
  2. 算子融合：conv+relu和flashattention大体都算这一类，两个算子的中间结果online到sram而不是写到内存里。
  3. kernel选择，针对特定的算子和特定的平台，可能不同的tile策略对性能的影响很大。同时根据实际测速，来选择最快的kernel实现。
  4. 量化。

有了tensorrt之后。。能做的东西就很少了。

这也启发我做了另外一个项目，pi.cpp: Robot Foundation Models inference in C/C++。

地图，数据闭环

在前面提到的三个场景中，第二个场景和其他场景有一个最大的不同，就是有地图。

这里不是讨论技术的先进与否，而是在某些场景下，有一个地图，确实是更好的交互界面。

给定一张照片，知道自己在地图的位置。其实是一个挺难的任务。

  1. 用现在的说法，他是一个image domain的reasoning任务。可以回忆一下我们是不是有过，到了一个地方，觉得眼熟，然后各种找细节，才能知道这里自己来过没有。

    这个问题的最经典的例子是：

  2. 这也是一个更好的我身边的例子，基础模型的发展，解决了垂直领域的问题。

简单来说，dino的CLS token已经是一个足够好的global feature。

所以我们使用这个feature做检索，再配合robust pose graph，建图就足够稳定了。

在AI的时代，依旧重视代码精炼

一方面，我觉得代码少，还是体现了某种审美和判断，对于系统中哪些是重要的，哪些是辅助性质的。

另一方面，确实也看到了一些代码，用几十万行甚至上百万行来实现我觉得可以几千行实现的功能。

面对这些代码，我试着去理解，试着用AI去改，但是都陷入了某种困境。

希望在不远的将来，AI能解决这些问题吧。