Planning-Tractor-Trailer路径规划

Tractor-Trailer是多刚体，强非线性系统，Trailer的运动轨迹是跟随运动，是由Tractor的轨迹决定的，如下图所示，Trailer和Tractor的行驶轨迹不同，因此在进行路径规划时，既要考虑Tractor的位姿，也需要考虑Trailer的位姿。

1. 规划方法

1.1 搜索方法

典型的搜索方法是Hybrid A*，需要对原始的Hybrid A*进行改进。在控制采样的过程中需要计算Trailer的姿态。此外当对Tractor-Trailer的末端时刻状态进行约束时，常规的RS曲线和Dubins曲线是基于car-like model的，因此需要对此进行改进。由于是采样搜索，是不完备的，其是否有可行解、求解时间和控制采样密度相关。并且此方法显然不适用与结构化道路。

2.2 优化方法

目前典型的最优化方法的路径规划，例如Apollo，可以利用道路中心线，快速求解。最优化方法可以方便的对任何位置施加约束。因此可以采用最优化方法求解Tractor-Trailer路径规划问题。

2. 问题模型

使用最优化方法进行路径规划时要考虑以下几个问题：

2.1 运动学约束

Tractor-Trailer的运动学公式如下：
$$\{\begin{array}{l}\dot{x}=v\cos \phi \\ \dot{y}=v\sin \phi \\ \dot{\phi }=\frac{v}{l}\tan \delta \\ \dot{\lambda }=\frac{v}{l_2}(-sin\lambda -\frac{d}{l_1}cos\lambda tan\delta )-\frac{v}{l_1}tan\delta \end{array}\text{\hspace{1em}(2-1)}$$

2.2 无碰撞约束

Tractor和Trailer都必须在无碰撞区域。

2.3 Two point约束

规划起点和终点的约束。

2.4 边界约束

各个优化变量的上下限约束。

3. 问题求解

由于约束是强非线性的，如果采用非线性求解器求解很难达到实时性要求，可以线性化后使用线性二次规划求解。

3.1 QP求解

由于进行了线性化引进了误差，会使求解结果不正确，例如下图Trailer会扫到第一个障碍物。

3.1 SQP求解

SQP是解决非线性规划的一种方法，如下图所示，可以避开所有障碍物。但SQP会增加求解时间。