自动驾驶算法岗笔试题 | 一道有意思的数学题 | 解析及代码实现

参考资料

• https://max.book118.com/html/2018/1105/8117074134001131.shtm
• 《一道运动学赛题的巧解及延伸》
• https://zhuanlan.zhihu.com/p/364295457

这两天参加了深圳一家企业的线下笔试，这个公司出了一道挺有意思的数学题，但是当时走马观花地看了下题目就直接写了，写完发现理解错了，不过又不想修改，所以直接交卷走人了。但是想想这道题还是挺有意思的，所以凭借着记忆尽量把这道题还原出来。

1. 题目描述

3辆飞船A、B、C初始位置分别处于一个边长为$L$的等边三角形的顶点上，即A点坐标为$(\frac{\sqrt{3}}{2}L,0)$，B点坐标为$(\frac{L}{2},0)$，C点坐标为$(-\frac{L}{2},0)$。飞船飞行过程中，飞船均具有相等的速度$v$，并且A始终朝着B前进，B始终朝着C前进，C始终朝着A前进。

• 问题1：从初始到$t_1$时刻，飞船A,B,C的运行轨迹（假设采样时间为$\Delta t$）。
• 问题2：到无穷远时刻时，飞船A,B,C最终的状态是怎样的？如果三者速度不相等，那么此时飞船A,B,C最终的状态又是怎样的？请推到分析。

2. 问题分析

1. 问题 1

对于问题1，当时错误理解成A、B、C在一开始的等边三角形的边上运动了，所以三下五除二就写出来了，很快啊（真的笑哭）。。

分析

由题意可知，三辆飞船因为速度相等，所以每时每刻飞船之间的距离都是相等的，即每时每刻都构成一个等边三角形，大致如图所示，他们依据某种螺旋曲线靠近。

我们从飞船A开始推导，其他两只飞船同理。假设$t$时刻飞船A的坐标为$P_A(t)$，那么根据向量的关系，很容易得到下一时刻飞船A的位置 为
$$P_A(t+1)=P_A(t)+n_{AB}(t)v\Delta t\text{\hspace{1em}(1)}$$

$n_{AB}(t)$为$t$时刻，飞船$A$指向飞船$B$的单位向量，它等于
$$n_{AB}(t)=\frac{P_B(t)-P_A(t)}{||P_B(t)-P_A(t)||}\text{\hspace{1em}(2)}$$
将等式(2)带入等式(1)，可得飞船A飞行的实时轨迹为
$$P_A(t+1)=P_A(t)+\frac{P_B(t)-P_A(t)}{||P_B(t)-P_A(t)||}v\Delta t\text{\hspace{1em}(3)}$$
同理，B、C飞船的轨迹为
$$P_B(t+1)=P_B(t)+\frac{P_C(t)-P_B(t)}{||P_C(t)-P_B(t)||}v\Delta t\text{\hspace{1em}(4)}$$
$$P_C(t+1)=P_C(t)+\frac{P_A(t)-P_C(t)}{||P_A(t)-P_C(t)||}v\Delta t\text{\hspace{1em}(5)}$$

根据 等式(3-5)，通过不停地迭代，即可求出从0时刻到$t_1$时刻的完整轨迹。

python代码实现

为了更好地画图展示效果，这边使用python编程。代码仓库见于github链接。

• 导入相关库

  import numpy as np
  import matplotlib.pyplot as plt
  from celluloid import Camera  # 保存动图时用，pip install celluloid
  
  %matplotlib qt5
  

• 参数设置

  # 采样时间
  dt=0.01
  # 原等边三角形长度
  L=1
  # 初始坐标点
  p_a = np.array([0,np.sqrt(3)/2*L])
  p_b = np.array([1/2*L,0])
  p_c = np.array([-1/2*L,0])
  # 速度
  v_a = 1.0
  v_b = 1.0
  v_c = 1.0
  
  
  
  p_center=np.array([0,1/np.sqrt(3)/2]) # 等边三角形中心
  
  # 存储轨迹
  trajectory_a=[]
  trajectory_b=[]
  trajectory_c=[]
  
  # 假设到t时刻停止
  t = 1
  
  
  

• 迭代公式

  def trajectory_point(p_1,p_2,v):
  n_ = (p_2-p_1)/np.linalg.norm(p_2-p_1)
  p_now = p_1+n_*v*dt
  return p_now
  

• 求解轨迹

  def all_trajectory(p_1,p_2,p_3,v_a,v_b,v_c):
      delta=0
      while(delta<t/dt):
          p__1=trajectory_point(p_1,p_2,v_a)
          p__2=trajectory_point(p_2,p_3,v_b)
          p__3=trajectory_point(p_3,p_1,v_c)
          delta+=1
          p_1,p_2,p_3=p__1,p__2,p__3
          trajectory_a.append(p_1)
          trajectory_b.append(p_2)
          trajectory_c.append(p_3)
      return p__1,p__2,p__3
  
  
  all_trajectory(p_a,p_b,p_c,v_a,v_b,v_c)
  
  trajectory_a=np.array(trajectory_a)
  trajectory_b=np.array(trajectory_b)
  trajectory_c=np.array(trajectory_c)
  
  
      
  

• 画图

  fig = plt.figure()
  camera = Camera(fig)  # 保存动图用
  for i in range(len(trajectory_a)):
      plt.cla()
  
      plt.plot([p_a[0], p_b[0]], [p_a[1], p_b[1]], '-.r', linewidth=1.0)
      plt.plot([p_b[0], p_c[0]], [p_b[1], p_c[1]], '-.r', linewidth=1.0)
      plt.plot([p_c[0], p_a[0]], [p_c[1], p_a[1]], '-.r', linewidth=1.0)
      plt.scatter(p_center[0],p_center[1])
      plt.plot(trajectory_a[0:i,0], trajectory_a[0:i,1], '-r',label="A")
      plt.plot(trajectory_b[0:i,0],trajectory_b[0:i,1],'-g',label="B")
      plt.plot(trajectory_c[0:i,0], trajectory_c[0:i,1], '-b',label="C")
      plt.legend()
      plt.xlabel('x/m')
      plt.ylabel('y/m')
      plt.axis('square')
      plt.grid(True)
      plt.pause(0.001)
      # camera.snap()
  
  
  # animation = camera.animate()
  # animation.save('trajectory.gif')
  
  plt.show()
  
  

运行轨迹如图所示：

2. 问题 2-1

其实我们可以很容易猜到，三辆飞船在速度相等的情况下，一定会在经过一段时间后三艘飞船相遇在原等边三角形的中心处，因为他们是高度对称的。那么是在什么时候相遇呢，可以看下面推导。

解法 1

根据题意，由于三个飞船都作等速率曲线运动, 而任一时刻 三艘飞船的位置都在正三角形的三个顶点上, 但这三角形的边长不断缩小, 如图所示。

现把从开始到追上的时间 $t$ 分成 $n$ 个相等的时间间隔 $\Delta t$, 在每个微小的时间间隔内, 每艘飞船的运动都可看成是直线运动. 经 $\Delta t,2\Delta t,3\Delta t,\cdots \cdots n\Delta t$, 对应 的三角形边长依次为 $L_1,L_2,L_3\cdots \cdots L_n$。当 $L_n\to 0$ 时三艘飞船相聚。

$A_1{B}_1$ 与 $A_1{B}_1^{\prime }$，差为二阶小量， 所以
$$L_1=A_1{B}_1\approx A_1{B}_1^{\prime }=L-A{A}_1-B{B}_1\cos 60^{\circ }=L-\frac{3}{2}v\Delta t$$

同理
$$\begin{gathered} L_2=L_1-\frac{3}{2}v\Delta t=L-2\frac{3}{2}v\Delta t, \\ {L}_3=L_2-\frac{3}{2}v\Delta t=L-3\frac{3}{2}v\Delta t, \\ \cdots \cdots L_n=L-n\frac{3}{2}v\Delta t \end{gathered}$$

当 $L_n\to 0$ 时三艘飞船相聚, 得 $t=n\Delta t=\frac{2L}{3v}$。 每艘飞船走过的路程 $s=vt=\frac{2L}{3}$.

解法2

因三艘飞船都作等速率曲线运动, 而任一时刻三艘飞船的位置都在正三角形的三个顶点上, 即速度沿三角形中心的分速度不变, 指向三角形中心的分速度
$$v^{\prime }=v\cos 30^{\circ }$$
沿三角形中心的位移
$$s^{\prime }=\frac{L}{2\cos 30°}$$
则时间$$t=\frac{s^{\prime }}{v^{\prime }}=\frac{2L}{3v}$$
从开始到相遇每艘飞船走过的路程为
$$s=vt=\frac{2L}{3}$$

3. 问题 2-2

如果三艘飞船的速度各不相等，那么很显然，最终相遇点不会是原等边三角形的中心。显然相遇点会偏向速度更慢的飞船。

例如，$v_A>v_B>v_C$，大致的运行轨迹如下所示：