Robust Trajectory Tracking Error Model-Based Predictive Control for Unmanned Ground Vehicles

In plain words

A farm tractor pulls a trailer, and it has to follow a path that is laid out against the clock: being in the right place at the right moment matters. A common approach is to predict where the vehicle will go and pick the steering that keeps it on track. Nonlinear versions of that predictor work well but are slow to compute. Simpler linear versions are fast, but they were built around straight-line driving and struggle on curves.

The authors write the tracking problem in terms of the difference between the planned path and the actual vehicle, which is the same idea as a feedback loop. A linear predictive controller works on that difference. A known feedforward command, computed from the planned path, does most of the work when the vehicle is on course. A third, robust correction pushes the vehicle back when it drifts. The three parts are added together, and the controller was tested on a real tractor-trailer in a field.

TL;DR

• Linear model predictive control (LMPC) tracks a time-based path for a tractor with a trailer, using a tracking-error model that also includes the speed dynamics §3.
• The control input is the sum of a feedforward term, the MPC output and a tanh-saturated robust term, which is a tube-based idea §4.
• Field tests on an 8-shaped path gave mean Euclidean errors of 21 to 40 cm, with the largest errors on the curves §5. Results are author-reported.
• The LMPC step averaged 1.1 ms, against 6.8 ms for an NMPC from earlier work on a different path parameterisation §5. This is not a like-for-like comparison.
• No code is public, and the card's pitfalls list several places where the equations are ambiguous or inconsistent.

Problem

Nonlinear MPC tracks well but its computation is expensive for real-time use. Linear MPC is cheap but, when linearised around a reference, models a curvilinear path poorly, and the model error grows as the vehicle moves away from the reference §1. Earlier tracking-error models for mobile robots take speed and yaw rate as inputs, which does not match how this tractor is actuated (steering angles and hydrostat position) §3. The paper assumes that the vehicle is kinematic: dynamic force balances are neglected §2. The feedforward design also assumes small steering angles and a steady-state speed response §4.2.

Method

System and model §2. The tractor and trailer are modelled kinematically, with a first-order speed model. The state is z = (x^t, y^t, \psi^t, x^i, y^i, \psi^i, v), the input is u = (\delta^t, \lambda, HP), where \delta^t is the front-wheel steering angle of the tractor, \lambda the angle between tractor and trailer, and HP the hydrostat position. The trailer steering angle is \delta^i = \lambda - \beta §4.1. The text uses L^d for a distance that the kinematic equations write as l §2.

Tracking-error model §3. The tracking error is z_e = T\,(z_r - z), where T is a block-diagonal rotation into the tractor and trailer frames §3. Linearising the error dynamics around the reference (all errors zero) gives a linear time-varying model \dot z_e = A z_e + B u_e. The coefficients depend on the reference speed v_r and the reference yaw rates \gamma^t_r, \gamma^i_r §3. The paper states that this model is controllable when v_r or a reference yaw rate is nonzero §3, Remark 1.

Feedback: MPC §4.1. The linear discrete model z_e(k+1) = A_d z_e(k) + B_d u_e(k) is solved online with prediction horizon N_p = 8 and control horizon N_c = 3 steps at 200 ms per step §4.1. The cost penalises the error states with Q and the input increments with R. Input and rate limits are applied as constraints. Only the first input increment is applied, and the resulting feedback is u_b = u^*_e §4.1.

# Feedback MPC step  [§4.1]
# Input:  error_state (7,), previous_input (3,)
# Output: feedback (3,)
def mpc_feedback(error_state, previous_input):
    # 1. Discrete model at the 200 ms sampling time  [§4.1]
    # The discretisation method is not stated.
    a_d, b_d = discretise(error_state_model)
    # 2. Optimise N_c = 3 increments over N_p = 8 steps  [§4.1]
    # Cost: sum of e^T Q e over 8 steps + sum of du^T R du over 3 steps
    increments = solve_qp(error_state, a_d, b_d, Q, R, limits)
    # 3. Apply the first increment on top of the previous input  [§4.1]
    return previous_input + increments[0]

Feedforward §4.2. The reference speed and yaw rates are computed from the reference path's first and second derivatives. The sign \pm in the speed equation selects forward or reverse driving §4.2. The feedforward inputs are

v_r = \pm\sqrt{(\dot x^t_r)^2 + (\dot y^t_r)^2} \qquad [\S 4.2]

\gamma^t_r = \frac{\dot x^t_r \ddot y^t_r - \dot y^t_r \ddot x^t_r} {(\dot x^t_r)^2 + (\dot y^t_r)^2} \qquad [\S 4.2]

\delta^t_r = \frac{\gamma^t_r L^t}{v_r}, \quad \delta^i_r = \frac{\gamma^i_r L^i - \gamma^t_r L^d}{v_r} - \beta, \quad HP_r = \frac{v_r}{K} \qquad [\S 4.2]

Robust term §4.3. The paper uses a tube-based MPC idea §4.3. The robust law is a saturated function of the position errors of the tractor and trailer and of their time derivatives. It is applied to the steering and hydrostat channels:

u_m = \begin{bmatrix} k_{s1} \tanh(k_{p1}\, y^t_m + k_{d1}\, \dot y^t_m) \\ k_{s2} \tanh(k_{p2}\, y^i_m + k_{d2}\, \dot y^i_m) \\ k_{s3} \tanh(k_{p3}\, x^t_m + k_{d3}\, \dot x^t_m) \end{bmatrix} \qquad [\S 4.3]

The gains are listed under Hyperparameters. The paper says the position-error vector z_m is kept in -1 \le z_m \le 1 §4.3.

Combined control §4. The applied input is the feedforward plus the robust term minus the feedback. Written as a sum: u = u_f - u_b + u_m §4.

# One control step  [§4]
# Input:  reference (state, inputs), measured_state (7,), previous_input (3,)
# Output: applied input (3,): steering of tractor, lambda, hydrostat
def control_step(reference, measured_state, previous_input):
    # 1. Tracking error, reference minus measured  [§3]
    error = frame_transform(measured_state) @ (reference.state - measured_state)
    # 2. Feedforward from the reference path  [§4.2]
    feedforward = feedforward_inputs(reference)
    # 3. Feedback from linear MPC on the error  [§4.1]
    feedback = mpc_feedback(error, previous_input)
    # 4. Robust tanh term on position errors and their rates  [§4.3]
    robust = robust_term(position_errors(error), position_error_rates(error))
    # 5. Combine: u = u_f - u_b + u_m  [§4]
    return feedforward - feedback + robust

Hyperparameters

Results

Author-reported. Setup: one time-based 8-shaped reference, made of two straight lines and two curves of radius 10 m, driven by a tractor-trailer on a field §5. Errors are the mean Euclidean distance to the reference. The baseline is an NMPC from earlier work by the same group, which used a space-based trajectory §5.

The baseline errors refer to a space-based path and the earlier paper's setup, so they are not directly comparable with the time-based errors here §5. The paper's conclusion is that the better approach depends on the shape of the path §5. The text reports no repeated runs and no spread across runs.

Paper vs code

No official code. The paper gives no code link, and the arXiv metadata has no code comment. Searched GitHub repositories for tractor-trailer mpc, tracking error mpc ground vehicle and kayacan mpc: the hits had unrelated owners and no link to the authors. Searched GitHub users for kayacan: the results were unrelated accounts, and none could be tied to the authors, so none was used. The implementation was in LabVIEW §2, which is not known to be public.

Pitfalls

• Feedforward for the trailer. The trailer feedforward \delta^i_r in §4.2 contains "-\beta", but the MPC input is \lambda §3 and the trailer steering is \delta^i = \lambda - \beta §4.1. Under small-angle kinematics, the expression without -\beta is (\gamma^i_r L^i - \gamma^t_r L^d)/v_r, which matches \lambda_r rather than \delta^i_r. This is the card author's reading of the equations; the paper does not say which quantity the feedforward is added to. Decide this explicitly and test it.
• Notation. The text defines L^d §2, but the trailer yaw equation writes the same length as l §2. Check that they are the same quantity before use.
• Units and rates. The constraint list gives rates in degrees per second and HP in %/s §4.1, but the optimisation problem gives HP limits without units. The model uses \tan(\delta), so the conversion from degrees to radians must be made explicitly. Units of R, of the tanh gains and of z_m are not stated.
• Discretisation and solver. The method for A_d, B_d and the QP solver are not stated §4.1, Remark 2.
• Time-varying model. A and B depend on v_r and \gamma_r §3, so they must be recomputed from the reference at each step, not fixed once.
• Robust term inputs. The robust law uses \dot x^t_m, \dot y^t_m, \dot y^i_m §4.3, but z_m is defined only as the four position errors §4.3. The paper does not say how the rates are estimated (filter or finite difference). The paper also says the states are estimated from measurements §4.3 without describing the estimator.
• Tube set. The paper uses the general tube form u_m = K(\bar z_e - z_e) §4.3 but then uses tanh gains. It does not compute the invariant set or the constraint tightening. The stability argument is left to the cited tube-MPC work §4.3.
• Feedforward preconditions. The reference must be twice differentiable, with v_r \ne 0 and K \ne 0 §4.2, Remark 3. The paper does not describe how the derivatives of the reference are computed.
• Ablation missing. The paper credits the robust term with reducing oscillation, and states that its contribution is larger on curves [§5, fig. 7]. It does not report a run without the robust term, so this claim is qualitative.
• Oscillation source. The trailer steering reference oscillates. The authors attribute this to the measured hitch angle \beta used in the feedforward §5.
• Cost hidden in the abstract. The 1.1 ms figure is an average on the PXI controller §5. The NMPC figure of 6.8 ms and the errors come from earlier work on a different path parameterisation §5. A fair comparison needs both controllers on the same path and hardware.
• Sign convention. The error is reference minus measured §3. The applied feedback is subtracted §4, so an implementation that uses the opposite sign in either place will fail.

Lineage

• Builds on: the tracking-error model idea of earlier mobile-robot work (Klancar and colleagues, cited as §3) and the tube-based MPC of Mayne and colleagues §4.3. This paper changes the model to include speed dynamics and the actuator inputs §3.
• Compared against: the NMPC and decentralised NMPC work of the same group on a tractor-trailer §5; the values are as quoted in this paper.
• Superseded by: not stated.