Tractor-Trailer is a multi-rigid body and strongly nonlinear system. The trajectory of the Trailer is following motion, which is determined by the trajectory of the Tractor. As shown in the figure below, the trajectories of the Trailer and Tractor are different. Therefore, when planning the path, both Considering the pose of the Tractor, the pose of the Trailer also needs to be considered.
1. Planning method
1.1 Search method
A typical search method is Hybrid A*, which needs to be improved on the original Hybrid A*. In the process of controlling the sampling, the attitude of the Trailer needs to be calculated. In addition, when constraining the end-time state of Tractor-Trailer, the conventional RS curve and Dubins curve are based on the car-like model, so it needs to be improved. Because it is a sampling search, it is incomplete. Whether it has a feasible solution, the solution time is related to the control sampling density. And this method is obviously not suitable for structured roads.
2.2 Optimization method
The path planning of typical optimization methods at present, such as Apollo, can use the centerline of the road to solve it quickly. Optimization methods can easily impose constraints on any location. Therefore, the optimization method can be used to solve the Tractor-Trailer path planning problem.
2. Problem model
The following issues should be considered when using the optimization method for path planning:
2.1 Kinematic constraints
Give the Tractor-Trailer Specifications:
{ x ̇ = v cos φ y ̇ = v sin φ φ ̇ = vl tan δ λ ̇ = vl 2 ( − sin λ − dl 1 cos λ tan δ ) − vl 1 tan δ (2-1) \begin{cases} \dot{x} =v \cos{\varphi} \\ \dot{y} = v \sin{\varphi} \\ \dot{\varphi} = \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{cases}\tag{2-1}⎩⎪⎪⎪⎨⎪⎪⎪⎧x˙=vcosPhiy˙=vsinPhiPhi˙=lvtandl˙=l2v( − s i n λ−l1dcosλtanδ)−l1vtanδ(2-1)
2.2 No Collision Constraints
Both Tractor and Trailer must be in the no-collision zone.
2.3 Two point constraints
Constraints for planning start and end points.
2.4 Boundary constraints
Upper and lower bound constraints for each optimization variable.
3. Problem Solving
Since the constraints are strongly nonlinear, it is difficult to meet the real-time requirements if a nonlinear solver is used to solve the problem. It can be solved by linear quadratic programming after linearization.
3.1 QP solution
Due to the error introduced by linearization, the solution result will be incorrect. For example, the Trailer in the figure below will scan the first obstacle.
3.1 SQP solution
SQP is a method to solve nonlinear programming, as shown in the figure below, all obstacles can be avoided. But SQP will increase the solution time.
