垂直泊车轨迹规划

• 垂直泊车规划简介
  垂直泊车即车辆由一定位置，通过横向运动和纵向运动，使得车辆由初始位置，最终垂直停到车位内。车辆检测完车位后，并停车结束后，需要对车辆进行轨迹规划。轨迹规划的目的是为了给出最佳的泊车入位路线，兼顾泊车时间小、泊车效率高、安全性高、舒适性好、轮胎磨损小等性能。根据规划轨迹，控制系统控制转向系统、制动系统和驱动系统沿着规划轨迹准确进入车位，同时避开障碍物。下图为一例垂直泊车时的轨迹。
  
  图1 垂直泊车轨迹规划例图
  泊车过程中曲线包括三种，直线、过渡曲线（渐变曲率）和圆弧。其中过渡曲线是为了避免原地打方向盘，减少轮胎磨损。通过合理的曲线计算，保证车辆顺利泊车入位。

• 垂直泊车计算相关理论
  2.1 一次进库计算方法
  车辆一次进库，即车辆由起始点位姿Init_Point通过一次后退停在库内Target_Point，中间没有停车换向。如下图所示，车辆先走一段直线，然后进入圆弧段，在车辆航向角与车位平行时直线后退至目标停车位，完成一次泊车入库。
  首先澄清关于以多大半径入库的问题，这里限定一次入库的圆弧半径为最小转弯半径，因为如果以其他半径能入库，那么以最小转弯半径也一定能入库，无非是切点P1，P2的位置不同。Target_Point位置为（0，l_r+Margin_Rear-L)），l_r为车辆后轴到车尾的距离，Margin_Rear为安全距离，L为车库位长度,W为车库位宽度，b为后轮距。
  根据前面文章提到的回旋线理论，直线与圆弧过渡应提前S/2转向，即得到提前打方向盘的位置点2和3。由于以最小半径去打方向盘，偏移的切垂距很小，可以忽略不计，所以得到如下图的泊车轨迹。
  整个过程应该满足以下条件：

• 点2在起始点1后边，即X_2 < X_1;

• 点P2在目标终点上边，即Y_P2 > Y_Target_Point;

• G点不碰左边界；

• H点不碰库边；

• 车辆不碰C点；
  
  图2 一次入库示意图
  下面对整个过程关键点计算方法进行详细介绍。
  2.1.1 关键点计算
  论证（1）（2）条件是否满足，要求出两个切点P1，P2的坐标。已知直线L1: a_1 x+b_1 y+c_1=0；直线L2: x=0，可以求出交点M坐标为：
  M（0，-c_1/b_1）,其中a_1=tan⁡(yaw_init ),b_1=-1,c_1=y_init-x_inittan⁡(yaw_init)
  即：
  M (0, y_init-x_inittan⁡(yaw_init))
  又∠P_1 MP_2 = π/2+yaw_init，则有
  ∠OMP_2= ∠OMP_1= (π/2+yaw_init)/2
  MP_1=MP_2=R_min cot⁡(∠OMP_1)
  OM=R_min/sin⁡(∠OMP_1)
  于是可以得到P_2的坐标为：
  P_2 (0, M_y-MP_2)
  P_1的坐标为：
  P_1 (MP_1cos⁡(yaw_init), MP_1sin⁡(yaw_init))
  圆心O的坐标为：
  O (R_min, P_2_y)
  根据回旋曲线理论可知，线段P_1-2，圆弧〖3-P〗_2的距离为S/2，S = Kφ。
  φ为最小半径对应的方向盘转角，因此，可以得到点2的坐标为：
  2 (P_1_x+S/2cos⁡(yaw_init), P_1_y+S/2*sin⁡(yaw_init))
  点3的坐标可以理解为点P_2绕圆心O旋转了S/2/R_min角度得到的新坐标，可以方便求出其坐标如下：
  3_x = O_x + (P_2_x- O_x) *cos(S/2/R_min) -(P_2_y- O_y) *sin(S/2/R_min)
  3_y = O_y + (P_2_x- O_x) *sin(S/2/R_min) + (P_2_y- O_y) *cos(S/2/R_min)
  3_yaw = π/2 - S/2/R_min
  3.1.2 碰撞干涉检查

• G点不碰左边界
  
  如图，G点不碰左边界的条件是：
  R_G+Margin<y_init-O_y

• H点不碰库边
  
  如图，H点不碰左库边的条件是：R_H+Margin<O_x+W/2

• 车辆不碰C点
  
  在整个泊车过程中，车辆后轴中心是距离库点C最近的点，为了避免车辆碰撞C点，要满足后轴中心的右边界点I不碰C点：
  
  3.2 三段式进库计算方法
  当车辆不满足一次入库的条件时，车辆需要在进入库内后至少再前后移动一次，才能倒库成功。
  。。。
  请关注微信公众号：自动驾驶原来如此，了解更多技术干货。扫码关注：