During my internship at Aethon Robotics, I got the opportunity to work on developing a controller for a tractor-trailer robot. I found the problem to be fascinating and decided to delve into the literature and try my hand at implementing something that would work for complex arrangements where the tractor trailer hitch point was offset behind the rear axle of the tractor head.
My research led me to find [1], which I found to be an elegant approach. I decided to implement the controller and contribute an open source code-base derived from this paper for anyone else interested in the same problem.
Motivation
This project was made as part of a larger path planning project for a tractor trailer assembly. Tractor-trailer are a natural choice when it is required to make the compute and actuation payload agnostic. This saves a lot of money as it allows the re-use of the tractor head on different payloads. Such assemblies are common place in the trucking industry. Moreover, the recent trend in vehicle autonomy might likely make its first and most important strides in trucking and heavy industrial vehicle automation.
Typically, these semi-trailer trucks have a single trailer but certain applications also make use of tractor heads attached to two or more trailers at a time, often the intermediate trailer acting as a steerable dolly for the trailer. While in forward motion, semi-trailers form a stable system. As the tractor head progresses along a path, the trailers converge to a steady state behind the head and track the path. In the worst case, trailers can cause larger off-tracking during turns.
During reverse motion, the tractor and trailer system quickly becomes unstable, i.e. for small changes in the tractor-trailer cart angle, the state of the system quickly diverges. Improper control while reversing can also lead to jack-knifing as shown in the figure below:
The controller implemented in this particular project was derived from the one presented in [1] with the following changes to it:
1. The kinematic model was re-derived for a single trailer system rather than the two trailer system shown in the paper
2. An LQR controller was used instead of the LQ controller (This is not too significant a change since the R term regulates control inputs which in this case is only the steering angle)
Kinematic Model
In order to derive the kinematic model for the assembly we first need to construct a representative diagram of the system. Most trailers are connected to the tractor at a hitch point that is located behind the rear axle of the tractor. The diagram shown below shows the hitch point located at a distance “M” behind the rear axle of the tractor. The tractor has a wheelbase of length “\(L_1\)” and the trailer has a wheelbase of length of “\(L_2\)”. These variables parametrize the physical aspects of the model.
The state of the model consists of the 2D location of the center of the rear axle of the trailer, the orientation of the trailer with the x-axis \(\theta\) and the the angle between the center-lines of the tractor and trailer starting from the tractor \(\beta\). The entire state being [x, y, \(\theta\), \(\beta\)]. The steering angle \(\alpha\) at the front of the tractor and the linear velocity \(v\) of the tractor’s rear axle form the control inputs of this system.
In order to derive the kinematic model for this assembly, let’s construct some additional geometry into the model remove the dimensions for clarity.
We know that the center point of the tractor’s axle moves forward with a velocity \(v\) and that the tractor steers with an angle \(\alpha\). The point “A” marks the hitch point between the tractor and trailer. Geometrically, the wheels of the tractor ought to trace concentric circles around a single turning center. For this to be true, the perpendicular projected from the steered wheel must intersect the extension of the rear axle. The turning center for the tractor is marked “\(O_1\)”.
Since the hitch point is part of both the tractor and trailer body, it must also trace a circle around point \(O_1\). Since it is also part of the trailer body, we can extend the radius joining \(O_1\) and point A to find the turning center of the trailer body. The turning center \(O_2\) is found by determining the intersection of the rear axle of the trailer and the extended turning radius of point A.
The state transition model must describe the change in the position (x,y) of the trailer, the change in the trailer orientation \(\theta\) and the tractor-trailer cart angle \(\beta\). If the velocity at the trailer rear axle center is \(v_B\), then we can write:
\[ \begin{aligned} \begin{bmatrix} \dot{x}\\ \dot{y}\\ \dot{\theta}\\ \dot{\beta} \end{bmatrix} = \begin{bmatrix} v_B\times \cos{\theta}\\ v_B\times \sin{\theta}\\ \omega_2\\ \omega_2 – \omega_1 \end{bmatrix} \end{aligned} \]
From the diagram constructed above, we can derive the following relations:
Given velocity \(v\) at the tractor axle and the steering angle \(\alpha\), the turning radius \(R_1\) is:
\[ R_1 = \frac{L_1}{\tan{\alpha}} \]
We also define the angle between \(v_a\) and the center-line of the tractor be \(\psi\). Then,
\[ \psi = \arctan{(\frac{M}{R_1})} \]
Where \(M\) is the tractor hitch offset from the first figure.
\[ \angle AO_2B = 180 – \psi – \beta \].
Now, from the tractor diagram we can write,
\[ \omega_1 = \frac{v}{R_1} \]
The velocity at point A, \(v_A\) is therefore
\[ v_A = \omega_1 \times R_2 \]
We can find \(R_3\) now from the relation
\[ R_3 = \frac{L_2}{\sin{(180 – \psi – \beta)}} = \frac{L_2}{\sin{(- \psi – \beta)}} \]
Using \(R_3\) we can now determine \(\omega_2\)
\[ \omega_2 = \frac{v_A}{R_3} \]
\(v_B\) can then be determined once \(R_4\) has been computed.
\[ R_4 = \frac{L_2}{-\cos{(\psi + \beta)}} \]
\[ ∴ v_B = \omega_2 \times R_4 \]
The final state transition matrix can then be written as:
\[\begin{bmatrix} \dot{x}\\ \dot{y}\\ \dot{\theta}\\ \dot{\beta} \end{bmatrix} = \begin{bmatrix} (\frac{v\tan{(\alpha)}M\tan{(\psi+\beta)}}{L_1\sin{(\psi)}})\times \cos{\theta}\\ (\frac{v\tan{(\alpha)}M\tan{(\psi+\beta)}}{L_1\sin{(\psi)}})\times \sin{\theta}\\ \frac{v\tan{(\alpha)}M\sin{(-\psi-\beta)}}{L_1 L_2\sin{(\psi)}}\\ \frac{v\tan{(\alpha)}M\sin{(-\psi-\beta)}}{L_1 L_2\sin{(\psi)}} – \frac{v}{L_1}\times \tan{\alpha} \end{bmatrix} \]
Control
As mentioned earlier, the system is unstable during backwards motion. However, there are certain equilibrium states for this model. Specifically, the equilibrium states arise when the tractor and the trailer turn about the same turning center. For control purposes we can stabilise around the equilibrium cart angles during backing up. In order to do this, we treat the cart angle state change equation as a sub-system and design an LQR controller around that sub-system.
In [1], a cascaded control approach is used. At the highest level, a pure-pursuit controller is used to directly compute the desired steering angle for forward motion and to compute the desired tractor-trailer cart angle during backward motion.
This leads to two different outputs from the pure-pursuit controller during forward and backward motion.
For computing the control inputs, the state matrix and control matrix are required. This can be obtained by linearizing the system about the quillibrium points.
The Jacobian with respect to the tractor trailer cart angle state ( \(\beta\)) of the state transition equation for the tractor-trailer cart angle is given by:
\[\frac{\partial \dot{\beta}}{\partial \beta} = (\frac{v \times tan(\alpha)}{L_1})\times(\frac{M}{L_2\sin{(\psi)}}\times(-\sin{(\psi)}\sin{(\beta)}-\cos{(\psi)}\cos{(\beta)})\]
The Jacobian with respect to the control input steering angle of the state transition equation for the tractor-trailer cart angle is given by:
\[\frac{\partial \dot{\beta}}{\partial \alpha} = (\frac{v \times tan(\alpha)}{L_1})\times(\frac{M^2\sin{(\beta)}L_1}{L_2\sin^2{(\alpha)}\sin^2{(\psi)}(R_1^2 + M^2)}) + (\frac{v}{L_1\cos^2{(\alpha)}})(\frac{M\sin{(\psi – \beta)}}{L_2\sin{(\psi)}} – 1)\]
With these equations, an LQR controller can be implemented to drive the tractor-trailer cart angle to the equilibrium state during reversing.
A diagram of the tractor-trailer at an equilibrium state is shown below for clarity. In this state, the rate of change of the tractor-trailer cart angle \(\beta\) converges to zero and is stable.
Results
Planned Motion Through a Cluttered Environment using a Sampling based Planner
Some of the resulting motions using the controller and a forward simulator are shown below. A fourth order Runge-Kutta integration was used to integrate the state with a time step of 0.001 seconds. The parameters of the vehicle were:
- Tractor Wheelbase = 0.3 meters
- Trailer Wheelbase = 0.5 meters
- Tractor Hitch Offset = 0.2 meters
- Velocity = 0.1 m/s
- Forward Lookahead Radius = 2 m
- Backward Lookahead Radius = 5 m
Backward motion:
K-Turn into A Free Space to Change Orientation
Some of the resulting motions using the controller and a forward simulator are shown below. A fourth order Runge-Kutta integration was used to integrate the state with a time step of 0.001 seconds. The parameters of the vehicle were:
- Tractor Wheelbase = 1.5 meters
- Trailer Wheelbase = 5 meters
- Tractor Hitch Offset = 0.5 meters
- Velocity = 0.3 m/s
- Forward Lookahead Radius = 4 m
- Backward Lookahead Radius = 5 m
Github Link to Source Code
References
[1] Niclas Evestedt, Oskar Ljungqvist and Daniel Axehill, Path tracking and stabilization for a reversing general 2-trailer configuration using a cascaded control approach, 2016, Intelligent Vehicles Symposium (IV), 2016 IEEE. http://dx.doi.org/10.1109/IVS.2016.7535535