平行泊车系统路径规划（4）

7 平行泊车路径规划

当车辆检测到目标车位后，系统需要根据车辆的起始点和泊车终止点进行路径规划，如果车辆起始点不在可行起始区域以内，就不能直接规划两点之间的路径，路径需要进行分段处理。从整体上来看，路径主要由两部分组成：第一段路径为车辆从起始点至可行起始区域内的路径；第二段路径为车辆从可行起始区域至泊车终止点的路径。
对于第一段泊车路径，是为了控制车辆行驶至可行起始区域内，以便进行后续的泊车操作。车辆在检测车位的过程中，通过传感器探测车辆与侧方其他停放车辆的距离，或是与车位线的距离，如果距离过小，可以根据探测距离实时调整车辆的横向位置。本文假设传感器探测到可用车位后，车辆的横向位置处于满足泊车条件的安全范围，主要考虑纵向方向。
对于第二段泊车路径，根据前文确定的路径曲线进行路径规划。由于路径曲线的改变，约束条件的增加，对应的可行泊车起始区域也将发生改变，因此需要对原始区域进行更新。
7.1 起始点区域划分
总体来讲，车辆起始点位置可分为两类：一种是在可行起始区域内；另一种是不在可行起始区域内。当车辆检测到目标车位后，可根据全球定位系统（Global Positioning System, GPS）、惯导（Inertial Measurement Unit, IMU）和视觉里程计（Visual Odometry, VO）等方向实现车辆的定位功能，由于本文将车辆定位部分作为已知输入条件，因此将车辆起始点的坐标信息作为已知条件，根据车辆的起始坐标进行判断车辆位于哪个区域。不同的起始点对应着不同的泊车路径，首先需要确定可行泊车起始区域的范围。
将上文的理论可行起始区域作为分析区域，设置车位尺寸为7m*2,2m ，根据车辆后悬长度考虑泊车完成后车辆后轴中心与车位之间的安全距离，设置泊车终止点坐标为（0.8,1.2），结合路径曲线表达式和约束条件，建立不等式约束方程，求解出满足泊车条件的起始点区域，结果如下图所示。

由上图可知，车辆起始点位置所在区域大致可划分为三类：区域1中的点都可直接与泊车终止点进行路径规划，区域1表示为可行泊车起始区域；区域2中的点首先需要规划一条沿 轴负方向到区域1的路径，然后再根据该路径的终止点和泊车终止点进行路径规划；区域3与区域2类似，只是路径方向为沿着 轴正方向进行规划。三个区域可近似表示为：

本节对应的matlab代码

7.2 不同起始区域的泊车路径
以泊车终止点为坐标系原点建立新的坐标系，对三个区域中各点坐标进行转化，针对不同区域分别进行路径规划：
（1）当车辆起始点位于区域1内。首先，根据等速偏移和正弦函数的叠加曲线路径表达式、车辆起始点坐标和终止点坐标，可得：

根据起始点和终止点处车辆需要保证与车位平行的姿态约束，可知路径在起始点和终止点处的一阶导数为零，可得：

另外在起始点和终止点处，车辆的前轮转角为回正状态，因此可以确定路径在起始点和终止点处的二阶导数也为零，可得：

根据上式可得：

根据上式可得：

根据上改写路径曲线表达式：

（2）当车辆起始点位于区域2内。根据起始点的纵坐标信息，可以在区域1中找到该纵坐标对应的横坐标范围。根据第一段路径的终止点和泊车终止点规划第二段路径。路径可表示为：

（3）当车辆起始点位于区域3内。区域3和区域2的路径规划相似，都是直线与拟合曲线的组合形式，在路径跟踪时需要增加一个判定条件，即当检测到车辆驶入区域1后，控制车辆按照区域1的规划路径行驶。

7.3 平行泊车库内姿态调整
上文中的泊车路径都是需要车辆一次泊车入位，对车位尺寸的要求较高，如果车位尺寸较小，路径将不能满足泊车要求。在实际情况中，经常会遇到较小车位的情况，车辆跟踪期望路径时只能跟踪路径的一部分，如果继续沿路径行驶，可能会与车辆后方物体发生碰撞，因此需要控制车辆向前行驶来调整车身姿态。
由于车辆未行驶到路径终止点，车身轮廓未完全进入车位，车位空间狭小，控制车辆向前行驶距离有限，因此，为了安全考虑，向前行驶采用圆弧路径，以车辆后轴中心最小转弯半径对应的圆弧作为库内姿态调整路径，如下图所示。当车辆进入车位后，传感器检测到车辆与车位后方边界的距离过近，车辆还未行驶到路径终止点，此时需要控制车辆以最小转弯半径向前行驶，当系统检测到车身已经处于车位中，泊车结束。为了泊车过程的安全性，虽然相比一次泊车入位情况，库内姿态调整存在原地转向的问题，但车辆沿第一段路径行驶时避免了原地转向，保证了路径的可跟踪性，降低了转向机构和轮胎的磨损。

7.4 有障碍物的泊车路径
在实际泊车过程中，有时会遇见突发情况妨碍泊车进行，如车辆沿着期望路径行驶时遇到障碍物，为了保证泊车过程的安全性，需要控制车辆进行避障。
一般情况下车辆避障的方式有两种：一种是当检测到原始路径上存在固定障碍物时，系统需要重新规划泊车路径，通过控制车辆转向的方式，使车辆沿着重新规划的路径行驶，达到避障的目的，如图（a）所示；另一种是当检测到临时动态障碍物时，系统控制车辆制动停车，等路径无障碍物后，再控制车辆继续按照原来的路径行驶，如图（b）所示。以上两种避障方法，需要根据不同的泊车环境进行判断选用。
针对静态障碍物，需要采用转向的方式进行避障，由于泊车的过程是一个小范围车辆运动控制，车位大小有限，周围可能还停放着其他车辆，如果路径上存在着固定障碍物，要实现车辆成功避障并完成泊车操作是十分困难的。针对动态障碍物，由于动态障碍物的运动不确定性，采用转向的方式可能无法成功实现避障，因此需要采用制动的方式进行避障。本文针对泊车过程中车辆后方出现行人、动物等动态障碍物的场景，选用制动避障方式，路径无需重新规划，车辆继续按照原始路径行驶。