赛车游戏中求解最短路径和最小曲率路径

最近参与了一个非常蛋疼的业余时间小项目：给定赛道和赛车模拟程序，求赛车跑完赛道的最快的办法。对于这个问题，我一开始的想法是：就像Google的自动驾驶一样，给定足够的训练数据，然后汽车对前方的画面做出判断决定当前时刻的驾驶策略。不过查了查文献，从游戏开发的角度，似乎一般都是用先找出赛车的最优路径，然后沿着路径驾驶就行了。我们最后采用的是后者，我负责了其中赛道产生中的一部分：近似求解最短路径(Shortest Path)和最小曲率路径(Minimum Curvature Path)。

给定的输入是赛道的图像文件，用OpenCV中的边缘检测模块得到赛道的线条和中心轮廓线：一堆按顺序排列好的像素点，描述了赛道的形状，输出则是最短路径和最小曲率路径按顺序排列的坐标点。

有了赛道轮廓之后，求解路径的办法主要采用的是[1]和[2]中描述的方法。

对赛道进行分段：

分段的办法是在轮廓中心线上取间隔相等的垂直线段，然后将赛道上的控制点坐标表示为在垂直线段上的相对位置。比如上面这幅图中的第i个线段，假设这个线段的上面端点叫\(P_{i,1}\)，下面的端点叫\(P_{i,0}\)，令\(W_{i}=P_{i,1}-P_{i,0}\)，则线段上任意一点\(P_{i}\)可以表示为：

\[\begin{align}
 & {{P}_{i}}={{P}_{i,0}}+{{\alpha }_{i}}{{W}_{i}}=\left( {{x}_{i,0}}+{{\alpha }_{i}}\left( {{x}_{i,1}}-{{x}_{i,0}} \right),{{y}_{i,0}}+{{\alpha }_{i}}\left( {{y}_{i,1}}-{{y}_{i,0}} \right) \right)=\left( {{x}_{i,0}}+{{\alpha }_{i}}\Delta {{x}_{i}},{{y}_{i,0}}+{{\alpha }_{i}}\Delta {{y}_{i}} \right) \\
\end{align}\]

这样做的好处是把需要用二维点坐标表示的赛道转化为一维的在赛道上的相对位置，如此一来需要处理的问题就变得简单而直观了。

最短路径：

来看\(P_{i}\)和\(P_{i+1}\)，两点间的距离为

\[\begin{align}
 & {{d}_{i}}=\left| {{P}_{i+1}}-{{P}_{i}} \right|=\sqrt{\Delta {{x}_{i+1,i}}^{2}+\Delta {{y}_{i+1,i}}^{2}} \\
\end{align}\]

其中，

\[\begin{align}
 & \Delta {{x}_{i+1,i}}={{x}_{i+1,0}}+{{\alpha }_{i+1}}\Delta {{x}_{i+1}}-\left( {{x}_{i,0}}+{{\alpha }_{i}}\Delta {{x}_{i}} \right)=\left( {{x}_{i+1,0}}-{{x}_{i,0}} \right)+\left[ \begin{matrix}
   \Delta {{x}_{i+1}} & -\Delta {{x}_{i}}  \\
\end{matrix} \right]\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
\end{matrix} \right) \\
\end{align}\]

\(\Delta y_{i+1,i}\)类似，就不写出来了。那么最短路径就可以看作是求下面这个优化问题：

\[\text{min: }\sum\limits_{i=1}^{n}{{{d}_{i}}^{2}} \]
\(\begin{align}
 & \text{subject to: }0\le {{\alpha }_{i}}\le 1 \\
\end{align}\)

注意到这里我们求的是\(\sum{{{d}_{i}}^{2}}\)而并非\(\sum{\left| {{d}_{i}} \right|}\)，为什么呢？当然不仅仅是为了避免求绝对值，将\(d_{i}^{2}\)展开我们得到：

\( {{d}_{i}}^{2}=\Delta {{x}_{i+1,i}}^{2}+\Delta {{y}_{i+1,i}}^{2} \)
\( ={{\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
\end{matrix} \right)}^{T}}\left[ \begin{matrix}
   \Delta {{x}_{i+1}}^{2} & -\Delta {{x}_{i+1}}\Delta {{x}_{i}}  \\
   -\Delta {{x}_{i+1}}\Delta {{x}_{i}} & \Delta {{x}_{i}}^{2}  \\
\end{matrix} \right]\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
\end{matrix} \right)+{{\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
\end{matrix} \right)}^{T}}\left[ \begin{matrix}
   \Delta {{y}_{i+1}}^{2} & -\Delta {{y}_{i+1}}\Delta {{y}_{i}}  \\
   -\Delta {{y}_{i+1}}\Delta {{y}_{i}} & \Delta {{y}_{i}}^{2}  \\
\end{matrix} \right]\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
\end{matrix} \right) \)
\( +2\left( {{x}_{i+1,0}}-{{x}_{i,0}} \right)\left[ \begin{matrix}
   \Delta {{x}_{i+1}} & -\Delta {{x}_{i}}  \\
\end{matrix} \right]\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
\end{matrix} \right)+2\left( {{y}_{i+1,0}}-{{y}_{i,0}} \right)\left[ \begin{matrix}
   \Delta {{y}_{i+1}} & -\Delta {{y}_{i}}  \\
\end{matrix} \right]\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
\end{matrix} \right) \)
\(\begin{align}
 & +\text{costant terms} \\
\end{align}\)

这个形式的表达式，正是标准的Simply bounded Quadratic Programming问题啊，于是求解\(\alpha\)就方便多了。至于为什么\(\sum{{{d}_{i}}^{2}}\)和\(\sum{\left| {{d}_{i}} \right|}\)基本等效，是因为在我们的问题中有个假设是赛道分割的宽度近似相等，关于这个假设后面还有进一步讨论。

最小曲率路径：

需要最小曲率路径的出发点是曲率越小，赛车能达到的最大速度越大，相应的关系[1]中有简单介绍。对于求解，这块的思路和最短路径思路类似，都是在一定假设下，先求局部量(曲率，距离)，找到形如\(ax+b\)的表达式，然后平方一下转化为QP问题进行求解。在[1]中求曲率使用的cubic spline，实际实现其实未必用得着(当然也是因为我比较懒……二次的实现比三次简单很多)，二次拟合和三次拟合求曲率的主要区别在于方向的影响，二次拟合求曲率时认为一个点周围的两个临近点是完全对称的，这和赛道分割的假设一致，所以可以用下面的公式来进行参数化：

\[\begin{align}
  & {{P}_{i}}={\mathbf{a}_{i}}+{\mathbf{b}_{i}}t+{\mathbf{c}_{i}}{{t}^{2}} \\
 & t=\frac{s-{{s}_{i}}}{{{s}_{i+1}}-{{s}_{i-1}}} \\
\end{align}\]

其中s是某一点到起点的路程，所以t的取值范围是-1到1。结合前面的假设来看只要分段等间隔可以被满足，那么\(\frac{dt}{ds}\)可以近似认为是个常数，经过一番和最短路径部分类似的繁琐推导，可以得到每一个路径点对应的曲率正比于\(\left| \mathbf{c} \right|\)。于是最小曲率路径又化成了一个QP问题：

\[ \text{min: }\sum\limits_{i=1}^{n}{{{c}_{i}}^{2}} \]
\(\begin{align}
 & \text{subject to: }0\le {{\alpha }_{i}}\le 1 \\
\end{align}\)

其中\(c_{i}^{2}\)的展开为：

\( {{c}_{i}}^{2}={{\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
   {{\alpha }_{i-1}}  \\
\end{matrix} \right)}^{T}}\left[ \begin{matrix}
   \Delta {{x}_{i+1}}^{2} & -2\Delta {{x}_{i+1}}\Delta {{x}_{i}} & \Delta {{x}_{i+1}}\Delta {{x}_{i-1}}  \\
   -2\Delta {{x}_{i}}\Delta {{x}_{i+1}} & 4\Delta {{x}_{i}}^{2} & -2\Delta {{x}_{i}}\Delta {{x}_{i-1}}  \\
   \Delta {{x}_{i-1}}\Delta {{x}_{i+1}} & -2\Delta {{x}_{i-1}}\Delta {{x}_{i}} & \Delta {{x}_{i-1}}^{2}  \\
\end{matrix} \right]\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
   {{\alpha }_{i-1}}  \\
\end{matrix} \right) \)
\( +{{\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
   {{\alpha }_{i-1}}  \\
\end{matrix} \right)}^{T}}\left[ \begin{matrix}
   \Delta {{y}_{i+1}}^{2} & -2\Delta {{y}_{i+1}}\Delta {{y}_{i}} & \Delta {{y}_{i+1}}\Delta {{y}_{i-1}}  \\
   -2\Delta {{y}_{i}}\Delta {{y}_{i+1}} & 4\Delta {{y}_{i}}^{2} & -2\Delta {{y}_{i}}\Delta {{y}_{i-1}}  \\
   \Delta {{y}_{i-1}}\Delta {{y}_{i+1}} & -2\Delta {{y}_{i-1}}\Delta {{x}_{i}} & \Delta {{y}_{i-1}}^{2}  \\
\end{matrix} \right]\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
   {{\alpha }_{i-1}}  \\
\end{matrix} \right) \)
\( +2\left( \left( {{x}_{i+1,0}}-{{x}_{i,0}} \right)-\left( {{x}_{i,0}}-{{x}_{i-1,0}} \right) \right)\left[ \begin{matrix}
   \Delta {{x}_{i+1}} & -2\Delta {{x}_{i}} & \Delta {{x}_{i-1}}  \\
\end{matrix} \right]\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
   {{\alpha }_{i-1}}  \\
\end{matrix} \right) \)
\( +2\left( \left( {{y}_{i+1,0}}-{{y}_{i,0}} \right)-\left( {{y}_{i,0}}-{{y}_{i-1,0}} \right) \right)\left[ \begin{matrix}
   \Delta {{y}_{i+1}} & -2\Delta {{y}_{i}} & \Delta {{y}_{i-1}}  \\
\end{matrix} \right]\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
   {{\alpha }_{i-1}}  \\
\end{matrix} \right) \)
\(\begin{align}
 & +\text{constant terms} \\
\end{align}\)

这种解法近似求得了全局曲率和的最小解，但是这个解对于赛车而言未必是最优的，因为求和值最小的解法，对于赛道这种分段数量庞大的情况，单个极值的影响总会被平均掉，仍很可能有的地方曲率特别大，导致成了整个赛道上保持高速的一个瓶颈。

更多关于等间隔假设：

基于最短路径和最小曲率路径的推导，可以看到等间隔假设在这种分段方法中非常重要。在最短路径中因为有了等间隔假设， 所以相邻的两段路径长度就被耦合在一起，不会出现相邻两段的路径差得很远的情况，所以最短路径推导中用\(\sum{{{d}_{i}}^{2}}\)才近似合理。而在最小曲率路径的推导中就更重要了，否则曲率项就变得非常复杂。然而这个假设是不是真的实用呢，其实还需要一个更强的假设，那就是每段之间的间隔远大于赛道宽度。

比如图中的例子，因为赛道间隔比较密，转弯半径特别小的时候，靠弯道内部分的间隔会很小，弯道外侧部分间隔很大，这使得最短路径和最小曲率路径的假设都受到影响，尤其是最小曲率路径，会得到很不好的结果，比如下图的最小曲率路径：

然而如果线段的间隔过大，当转弯很急的时候，采样点的数量会显得不够。对于这种情况，我解决的办法是将分段的线段分组，比如每隔一个取一个分段线段，相当于得到了错位开来的两组分段线段，每组的间隔都是原来分段间隔的二倍，然后做两次路径求解，取平均，当然也可以加入一些后处理，比如向上重采样然后做平均平滑等等，结果在下面一节的图里可以看到。不过即便采用了一些手段来确保产生合理的赛道，对于最小曲率路径而言，这还是一个trade off的问题，因为间隔越大的情况对急转弯的采样会越差，如果特别急的弯道还会因为漏采样出现“撞赛道”问题，需要更多后处理。总结来说，就是这种基于QP的算法，无论二次还是三次，都不能很好应对急转弯，不过在实际的赛车游戏当中，像上面图里的那种急转弯相对来说不是特别常见，所以用起来效果还可以，需要考虑的往往是在赛道预处理、分段和QP时因为采样过密导致的时间和内存消耗。

基于最短路径和最小曲率路径的最优赛道：

有了最短路径和最小曲率路径后，求最佳路径的办法可以参考[2]，大体描述如下：首先找到最短路径和最小曲率路径的每个交点，将两个交点之间的划为一组，这样相当于降维，把优化所有的\(\alpha\)转化为优化好几组\(\alpha\)。对于第j组分组中，\(\alpha_{j}\)的值表示为最短路径的\(\alpha_{j,sp}\)和最小曲率路径的\(\alpha_{j,mcp}\)的加权平均：

\[\begin{align}
 & {{\alpha }_{j}}=\varepsilon {{\alpha }_{j,sp}}+\left( 1-\varepsilon  \right){{\alpha }_{j,mcp}} \\
\end{align}\]

比如下图是令\(\varepsilon\)为0.5时得到的一个路径，和对应的最短路径、最小曲率路径，以及分段。

对得到的路径进行模拟，得到跑圈时间，于是转化成一个全局优化的问题：

\[ \text{min: lap time} \]
\(\begin{align}
 & \text{subject to: }0\le {{\varepsilon }_{j}}\le 1 \\
\end{align}\)

[2]中用的是基因算法(Genetic Algorithm)，其实如果赛道不是很复杂的话，分段应该不多，低维情况可以考虑用比GA收敛快的算法。

如何沿指定赛道驾驶：

这部分我没有参与，也没有进行过调研，应该也没有时间去参与了。不过我觉得一个也许可行的办法是将赛道上每一点对应的最大速度和该点到起点的距离表达出来\(v_{\max }\left( s \right)\)，最大速度的求法根据不同情况会有不同答案，对于一般的赛道而言，近似认为最大速度被转弯离心力和风阻限制，相应的公式[2]中有提到。另外从任意速度开始以恒定加速度进行加速和减速也可以得到一个以该点为起点距离为变量的速度表达式\(v\left( r \right)\)(似乎大约是个\(\propto \sqrt{r-{{r}_{0}}}\)的关系)，那么又转化为一个优化问题：

\[ \text{max: }\int{v\left( s \right)} \]
\(\begin{align}
 & \text{subject to: }v\left( s \right)<{{v}_{\max }}\left( s \right) \\
\end{align}\)

这个问题甚至也许不需要优化就能求出解析解。

参考文献：

[1] F. Braghin, F. Cheli, S. Melzi, and E. Sabbioni. Race driver model. Comput. Struct., 86(13-14):1503–1516, 2008.

[2] Cardamone, L., Loiacono, D., Lanzi, P.L., & Bardelli, A.P. Searching for the optimal racing line using genetic algorithms. In Proceedings of the 2010 IEEE Conference on Computational Intelligence and Games (pp. 388–394).