听说现在自动驾驶很火，所以我也做了一个

雷锋网(公众号：雷锋网)按：本文作者qqfly，上海交通大学机器人所博士生，本科毕业于清华大学机械工程系，主要研究方向机器视觉与运动规划，个人微信公众号：Nao（ID:qRobotics），雷锋网授权发布。

最近，天气变得越来越冷，实验室也就变得越来越远。就连像我这样比较勤奋的博士生都没办法准时到实验室了。

短短的2.2km，成为了我学术道路上最大的绊脚石——一想到要在寒风中骑15分钟自行车，就根本不想起床了。

现在，有了「××」分时租赁电动汽车（显然没有广告费），开开心心就能到实验室。一口气开两公里，不会冷。

但是，用过几次分时租赁之后，问题出现了。

没错，那就是附近没有车！尤其是像我这样比较勤奋的博士生，经常学习（刷知乎）到深夜，那样就只能默默走2.2km回宿舍了。

「要是预定之后，汽车能自动开到你附近就好了。」我不禁这样想。

这不就是二维路径规划问题嘛，对于一个知道四十种运动规划算法的博士生，这个完全是道送分题。

要是做出来了，那岂不是立马制霸分时租赁市场，再随便忽悠个风投，然后把「××」打车收购了，改成「××」自动驾驶出租车。想想都激动。

利益熏心之下，我立马投入了工作。由于平时见多识广，我很快就找到了解决这个问题的办法：

• 1）写一个全局规划器，让电动车能在任意两个停车点之间找到开车路线；

• 2）写一个局部规划器，控制电动车跟随全局轨迹，并实现避障等功能。

So easy!

由于每个停车点相对固定、校园环境也比较简单，所以全局规划器并不复杂，将校园地图做成网格（或拓扑图），跑一个A*就可以了。当然，毕竟我是机动学院的，所以我做的这种车也只能走机动车道，为了防止它跑出机动车道，可以加一个costmap。不多说。

costmap示意图，让距离不可行区域（如非机动车道）较近的路径耗费更高即可

写完全局规划器，就只剩下局部规划器了。

这个局部规划器也简单，看了文献，大概有个叫做Timed-Elastic-Band的多目标优化方法，只要写出需要满足的优化目标函数即可：

算了，差不多这样就可以了，出于严谨的科学态度，我用stage搭了一个仿真平台，测试这个算法。

哎呦，不错哟！

那就直接上真车吧。但毕竟「××」分时租赁不肯赞助我，我没办法用他们的车做实验，我只能把视线投向了实验室的移动小车。

算了，我还是去当电影剪辑师好了。

===============

不好意思，上面都是临时工写的。为了不剧透，临时工剪辑的视频也放在最后。

由于之前一直宣扬「path planning 跟 motion planning 在数学上是一个问题」，于是被拉入坑，让我去做这个自动驾驶车的东西。

当然，一开始让我去做这个路径规划，其实我的拒绝的。

但后来一想，正好能在低维情况下试试那些基于优化的路径规划算法，然后顺便再发篇论文，也不错。

全局规划器跟上面临时工说得差不多，就是costmap + graph search。costmap就是根据地图的可行区域，再通过耗费函数计算出来的一个图。即，

cost function + map = costmap

重点是局部规划器。

由于，局部规划器所需满足的约束条件比较多，就可以通过设计一堆由路径决定的优化目标函数，利用优化算法对它求解进行了。

对于二维路径的描述，有一个有趣的方法，叫做Elatic Band（橡皮筋）。

简而言之，就是连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。

先定义一下我们的橡皮筋：

起始点、目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的控制点（机器人姿态），当然，为了显示轨迹的运动学信息，我们在点与点之间定义运动时间Time。于是，这个方法就叫做Timed-Elastic-Band。即

time + elastic band = timed elatics band

之后就需要定义「内外力」的数学表达形式，即目标函数。（以下均为中学知识）

1) 要跟踪全局轨迹+要避开障碍物：

这两个其实算是一类问题，都是在橡皮筋上找到距离某一点（全局路径点/障碍物）最近的状态，计算两者之间距离，之后定义一个基于距离的势场就好了。

当然，上面两种目标函数随距离变化方向正好相反，一个随着距离增大而增大（跟踪），一个随着距离增大而减小（障碍物）。

2) 加速度/速度限制

这个其实就是一个不等式约束。

我们的橡皮筋只定义了姿态（x,y,θ）与两两状态直接的时间，所以就直接用差分近似计算好了。

3) 运动学限制

这个比较重要，毕竟我们一般都不希望自己的车漂移起来。

所以，我们的控制量只有车速（油门）与转角（方向盘）。

我们假设两个状态点之间转角相同，（如果发生了转向，中间加状态点就好了）。

简单的向量叉乘求夹角即可

当然，由于汽车的结构限制，汽车会有一个最小转弯半径。（毕竟汽车不能原地转弯）

4) 当然，如果有其他约束也可以扔进去

目标函数都定好之后，就需要进行求解了。

这么复杂的多目标优化问题，一看就不想做

好吧，这步虽然看似复杂，但是了解SLAM或者SFM的同学应该能很快反应过来，这就是一个bundle adjustment问题。

简而言之，虽然待优化的橡皮筋有不少状态点与时间段，目标函数也好像很多。但是，每个目标函数只与橡皮筋中的某几个状态有关，而非整条橡皮筋。认识到这是一个稀疏优化（Sparse Optimization）问题就比较容易了。

将它描述成图，然后用图优化。

如图，这个图的节点vertexs是橡皮筋的状态（机器人姿态+时间）；图的边edges是我们自己定义的优化目标函数。

求解的框架，自然是可以去使用g2o（A General Framework for Graph Optimization）了。当然，节点和边的类型需要我们自己利用g2o中的模板定义。

最后是临时工剪辑的视频Cost Map Elastic Band，不能只有我一个人被洗脑：

https://v.qq.com/x/page/c0354qk9qne.html

雷锋网特约稿件，未经授权禁止转载。详情见。