赛车游戏中求解最短路径和最小曲率路径_.NET_编程开发_程序员俱乐部

最近参与了一个非常蛋疼的业余时间小项目：给定赛道和赛车模拟程序，求赛车跑完赛道的最快的办法。对于这个问题，我一开始的想法是：就像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}\)可以表示为：

\[{{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)\]

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

最短路径：

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

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

其中，

\[\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)\]

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

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

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

\[\begin{align}
  & {{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) \\
 & +\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)+\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) \\
 & +\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}}={{a}_{i}}+{{b}_{i}}t+{{c}_{i}}{{t}^{2}} \\
 & t=\frac{s-{{s}_{i}}}{{{s}_{i+1}}-{{s}_{i-1}}} \\
\end{align}\]

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

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

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

\[\begin{align}
  & {{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) \\
 & +\left( \left( {{x}_{i+1,0}}-{{x}_{i,0}} \right)-\left( {{x}_{i,0}}-{{x}_{i-1,0}} \right) \right)\left[ \begin{matrix}
   2\Delta {{x}_{i+1}} & -4\Delta {{x}_{i}} & 2\Delta {{x}_{i-1}}  \\
\end{matrix} \right]\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
   {{\alpha }_{i-1}}  \\
\end{matrix} \right) \\
 & +\left( \left( {{y}_{i+1,0}}-{{y}_{i,0}} \right)-\left( {{y}_{i,0}}-{{y}_{i-1,0}} \right) \right)\left[ \begin{matrix}
   2\Delta {{y}_{i+1}} & -4\Delta {{y}_{i}} & 2\Delta {{y}_{i-1}}  \\
\end{matrix} \right]\left( \begin{matrix}
   {{\alpha }_{i+1}}  \\
   {{\alpha }_{i}}  \\
   {{\alpha }_{i-1}}  \\
\end{matrix} \right) \\
 & +\text{constant terms} \\
\end{align}\]

这种解法近似求得了全局曲率和的最小解，但是这个解对于赛车而言未必是最优的，因为平均值最小时，仍很可能有的地方曲率特别大，导致成了整个赛道上保持高速的一个瓶颈。

更多关于等间隔假设：

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

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

然而如果线段的间隔过大，当转弯很急的时候，采样点的数量会显得不够。对于这种情况，我解决的办法是将分段的线段分组，比如每隔一个取一个分段线段，相当于得到了错位开来的两组分段线段，每组的间隔都是原来分段间隔的二倍，然后做两次路径求解，取平均，当然也可以加入一些后处理，比如向上重采样然后做平均平滑等等，结果在下面一节的图里可以看到。

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

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

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

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

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

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

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

如何沿指定赛道驾驶：

这部分我没有参与，也没有进行过调研，应该也没有时间去参与了。不过我觉得一个也许可行的办法是讲赛道上每一点对应的最大速度和该点到起点的距离表达出来\(v_{\max }\left( s \right)\)，另外从任意速度开始以恒定速度进行加速和减速也可以得到一个以该点为起点距离为变量的速度表达式\(v\left( r \right)\)，那么又转化为一个优化问题：

\[\begin{align}
  & \text{max: }\int{v\left( s \right)} \\
 & \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).