Research Project in Mechatronics Engineering

Part IV Project Report

Model Predictive Control for Path Tracking a Mobile

Robot in a Remote Lab

Eloise Beattie

Project Report ME069-2024

Co-worker: Tamsin Holmes

Supervisor: Dr Hazim Namik

13 October 2024
ME069-2024

Model Predictive Control for Path Tracking a Mobile

Robot in a Remote Lab

Eloise Beattie

ABSTRACT

Mechatronics and Mechanical Engineering at the University of Auckland are highly practical
disciplines requiring real-world experience to truly grasp concepts. A specific course these
students undertake is MECHENG 201, which requires practical implementation of control
systems on a robot. This course is challenging and requires large periods of time spent
manually tuning and iterating the control system within the lab. Unfortunately, the lab has
limited access availability which can create stress and a poor course experience. While there
is simulation software available to test the overall control architecture, this is not a direct
replacement for real-world testing due to physical phenomena such as slip and backlash.
This problem within the course has lead to the development of a remote laboratory allowing
students to trial their code on a physical robot from any location while offering a wider
range of available time slots. The construction of the remote lab has a few necessary
requirements to ensure the system actually benefits students. The one most relevant to this
project is the ability to move the robot to any location within the designated area at a
desired orientation allowing for consistent starting positions from any point. This requires
the use of a path tracking system enabling the robot to navigate efficiently whilst avoiding
any obstacles. This system is currently implemented using a pure pursuit controller which
lacks orientation control and consistent accuracy.
As such, this project concentrates on improving the path tracking capabilities of the robot
and therefore, enhancing the user experience of the University of Auckland remote labs.
To identify the most promising solutions and better advise the scope of this project, an
analysis of available literature was completed. This focused on researching relevant path
tracking implementation methods specifically designed for a rear-wheel differential drive
robots. This literature review revealed that Model Predictive Control (MPC) was both a
viable and highly relevant method of navigation due to its ability to predict future states and
react accordingly similar to that of how humans drive a car. With this in mind, the research
objectives for this project were identified as the following: Exploration and simulation of
MPC assessing its potential in improving the system, implementing the designed controller
on the remote lab hardware, evaluation of success using defined performance metrics and
overall assessment of the benefits in regards to the remote lab. The methodology designed
to fully encompass these objectives included implementing the controller first in MATLAB
and conducting investigations into the systems nature and how different parameters affected
the response. This was trialled on a model designed to represent the kinematics of the
robot and compared with the pure pursuit controller tested on the same model. These
results informed the hardware implementation and lead to the full development of an MPC
controller in the remote lab.

ii
Final testing and evaluation demonstrated significant improvements in path accuracy and
the addition of orientation control when compared with pure pursuit. This aims to provide
immense value to students allowing consistent and reliable control of the robot and ideally
benefiting their overall education at the University of Auckland. Further value lies in the
potential future implementation of utilising the MPC controller for path planning in addition
to tracking ideally improving efficiency and tracking in future iterations.

iii
 DECLARATION

Student
Flose Berettie
I ………………………………………………….. hereby declare that:
1. This report is the result of the final year project work carried out by my project partner (see
cover page) and I under the guidance of our supervisor (see cover page) in the 2024 academic
year at the Department of Mechanical Engineering, Faculty of Engineering, University of
Auckland.
2. This report is not the outcome of work done previously.
3. This report is not the outcome of work done in collaboration, except that with a project
sponsor as stated in the text.
4. This report is not the same as any report, thesis, conference article or journal paper, or any
other publication or unpublished work in any format.
In the case of a continuing project: State clearly what has been developed during the project and what
was available from previous year(s):
Available to us from previous years was:
• All necessary hardware for the VEX V5 version of the remote lab.
• A computer vision system for robot localisation within the arena using an Extended Kalman
Filter.
• Supervisory code ensuring the robot remained within defined boundaries.
• Pure pursuit functionality for path tracking.
• The GUI on the Laboratory Computer for robot interaction and integration with the
University's PC booking system
• Path Planning using an A* algorithm integrated with GUI
Work developed during the project:

• Model Predictive Control algorithm for path tracking

o Simulated in MATLAB
o Implemented on hardware setup available from previous years
• Communication system between Raspberry Pi, VEX robot and Laboratory Computer
developed for compatibility with Model Predictive Control
D
Signature: _________________________________
Date: 10/10/2024
Supervisor
I confirm that the project work undertaken by this student in the 2024 academic year is / is not
(strikethrough as appropriate) part of a continuing project, components of which have been
completed previously.
Comments, if any:

Signature: _________________________________
Date: 11/10/2024
______________

iv
Table of Contents
Acknowledgements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vi

Glossary of Terms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii

Abbreviations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii

1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

2 Literature Review . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
2.1 Path Tracking Methodologies . . . . . . . . . . . . . . . . . . . . . . . . . . 2
2.2 Model Predictive Control in Literature . . . . . . . . . . . . . . . . . . . . . 2
2.3 Application of Model Predictive Control for Differential Drive Robots . . . 3
2.3.1 Common MPC Models Relevant to DDMRs . . . . . . . . . . . . . 4
2.3.2 Software Tools for MPC . . . . . . . . . . . . . . . . . . . . . . . . 5
2.4 Literature Review Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3 Project Scope and Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

3.1 Research Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

4 Design Methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

5 Model Predictive Controller Design and Implementation . . . . . . . . . . . . 7

5.1 Robot Plant Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
5.2 Objective Function and Optimisation . . . . . . . . . . . . . . . . . . . . . . 8
5.3 Path Tracking Methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
5.3.1 Path Construction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
5.3.2 Waypoint Iteration Method . . . . . . . . . . . . . . . . . . . . . . . 10
5.3.3 Intermediate Waypoint Path Tracking . . . . . . . . . . . . . . . . . 11
5.4 Hardware Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

6 Parameter Optimisation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
6.1 Horizon Length . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
6.2 State Weighting . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
6.3 Control Weighting . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
6.4 Path Weighting . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

7 Pure Pursuit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

8 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
8.1 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
8.2 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

9 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

10 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

11 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

Appendix A Appendix A: Python Script for MPC Class . . . . . . . . . . . . . 27

v
Acknowledgements

I would like to extend sincere gratitude to Dr. Hazim Namik (Department of Mechanical
and Mechatronics Engineering) for his invaluable guidance and support as this projects
supervisor. His quality feedback and expertise allowed this project to be completed to
a standard I am proud of and has fostered a genuine interest in the field I would not
have otherwise had. He constructively challenged and questioned me throughout the design
process and I find I am a better engineer because of it.
I would also like to thank the lab technician Sophia Zhang and the IT Department at the
University of Auckland for their support and tolerating the many questions and hardware
challenges faced.
Finally, I would give my most heartfelt thank you to my project partner Tamsin Holmes.
She gave valuable insights and dedication to the completion of this project, whilst also
staying determined even when we faced significant challenges. Her support throughout this
entire year was invaluable and I could not think of a better partner.

vi
Glossary of Terms

Term Definition
MATLAB MATLAB is a proprietary multi-paradigm programming
language and numeric computing environment developed by
MathWorks.
Simulink Simulink is a MATLAB-based graphical programming
environment for modelling, simulating and analysing
multi-domain dynamical systems.
CasADi CasADi is an open-source library used for non-linear
optimisation and algorithmic differentiation.
ARUCO Marker 2D binary-encoded fiducial patterns designed to be quickly
located by computer vision systems
State A set of variables used to define a system at one time instant
VEX Robotics brand specialising in education
PROS Open Source C/C++ development tool for VEX V5 and VEX
Cortex
Open CV Library of programming functions for real-time computer
vision analysis
Backlash Motion lost in a motor caused by gaps between gears or parts
Slip Slip of wheels against ground resulting in a difference between
position travelled and wheel motion
Serial Communication method used to transmit data

Abbreviations

MPC Model Predictive Control

DDMR Differential Drive Mobile Robot
EKF Extended Kalman Filter
MIMO Multiple Input Multiple Output
FSFB Full State Feedback
LQR Linear Quadratic Regulator
RSC Robust State Feedback Control
PI Proportional-Integral
PID Proportional-Integral-Derivative
GPC Generalised Predictive Control
LMPC Linear Model Predictive Control
NMPC Non-Linear Model Predictive Control

vii
1. Introduction

The Faculty of Engineering at the University of Auckland is committed to providing Mechat-

ronics and Mechanical Engineering students with hands-on learning experiences that are
crucial for mastering the practical applications of engineering principles. In particular,
comprehension of control systems requires a blend of theoretical knowledge and practical
application due to real-world challenges such as slip and backlash, which are often not
fully replicable in simulations.
In the second-year course, MECHENG 201, students are tasked with utilising their control
system knowledge to control a VEX robot. Feedback for this course indicates that students
encounter significant challenges due to restricted access to laboratory resources, which can
impede their ability to develop and effectively test control systems. This constraint has led
to the development of a remote lab concept, enabling students to test and refine their control
algorithms on an actual robot from any location. This remote setup not only alleviates the
stress associated with limited physical lab time but also enhances the learning process by
providing real-time, real-world testing environments. For this system to be effective, the
ability for students to remotely move the robot to the location and orientation necessary to
start their code is essential.
The existing remote lab path tracking system lacks robust orientation control and shows
mediocre positional accuracy. This project will focus on refining these aspects by enhancing
the "go-to" functionality, which is vital for navigating the robot to precise locations and
orientations without manual intervention. The integration of a Model Predictive Control
(MPC) strategy, known for its ability to forecast future states and adjust controls pre-
emptively, aims to significantly improve both path tracking and orientation capabilities of
the laboratory robots.
This project aims to improve upon the existing system by implementing a modern controller,
such as MPC, that allows for increased controllability, accuracy and efficient. The success
of this project should thereby optimise the learning outcomes and resource utilisation in
lab settings.

2. Literature Review

This section provides a critical analysis of path tracking methodologies, with a particular
focus on the application of MPC, relevant to differential-drive mobile robots (DDMRs). To
better inform the scope and methodologies implemented, this analysis explores several key
areas:

1. Path Tracking Methodologies

2. Model Predictive Control in Literature

3. Application of Model Predictive Control for Differential Drive Robots

The purpose of this literature review is to guide the development and enhancement of
a path tracking system for the VEX robot, specifically adapted for the University of
Auckland’s remote labs. By delving into these targeted areas, the project aims to optimise
the functionality and educational value of the remote lab environment, ensuring that students
gain hands-on experience with advanced, real-world control systems.

1
Background Ensuring compatibility within the remote lab was essential for the application
of the control system as any controller implemented is highly dependent on the system
structure. This setup includes the use of a rear-wheel differential drive robot controlled
by the VEX V5 microcontroller. A C++ open source development plug-in called PROS is
used for software development containing inbuilt speed control. State feedback is received
through the use of a camera vision system in conjunction with Open CV before being
transferred to the robot and corrected by the internal Extended Kalman Filter (EKF).
This setup was essential to keep in mind throughout research to ensure options would be
compatible and feasible for this environment.
2.1 Path Tracking Methodologies

Path or trajectory tracking for DDMRs is extensively researched due to its wide range
of applications, particularly in agricultural and industrial sectors. DDMRs, characterised
by constrained movement, Multi-Input Multi-Output (MIMO) capabilities, and non-linear
dynamics, necessitate a range of control strategies. The literature highlights several com-
monly used methods, including Full-State Feedback (FSFB) controllers like the Linear
Quadratic Regulator (LQR) and Robust State Feedback Control (RSC). Other significant
control methods found include neural network-based control, simple proportional-integral
(PI) controllers, MPC, and Pure Pursuit algorithms, each offering unique advantages based
on the system’s specific requirements and complexities [1].
Currently, a pure pursuit controller is employed on the VEX robot. Both literature and
trials within the remote lab itself indicate that although it outperforms the PI controller,
it faces challenges with robustness due to the inflexible look-ahead distance which is a
common theme for traditional pure pursuit implementation [2]. This limitation manifests as
substantial tracking errors during complex path manoeuvrers. Furthermore, this approach
does not allow direct control over the final orientation of the DDMR, a critical feature for
this project [3, 4].
FSFB controllers generally perform well in managing MIMO systems, but their effectiveness
can be compromised by system non-linearities, necessitating linearisation which may reduce
tracking accuracy [5]. Comparative studies between MPC and RSC show that while MPC
offers better accuracy and quicker response times as represented in Figure 1, it may become
unstable under adverse external conditions [6]. Given that this project’s application is within
a controlled indoor environment, such instability concerns are not relevant to this projects
scope. The potential for implementing neural networks is also not within this projects scope
and was not considered as an option for implementation.
These results show evidence to support that MPC is well-suited for the control of a DDMR
in the application of path tracking, warranting further investigation.
2.2 Model Predictive Control in Literature

Model Predictive Control does not designate a specific control strategy but instead utilises
a plant model to optimise the control signal according to a predefined objective. This
strategic use of a plant model allows the controller to forecast the systems future outputs
over a defined horizon allowing for the optimal control signal to be sent to the plant. This
predictive capability is foundational to all MPC frameworks, which invariably include a
plant model, objective function and optimisation method. The general method of optimising
the control effort is shown in Figure 2 where future outputs for a determined horizon, N ,
called the prediction horizon, are predicted at each instant t using the plant model. Each

2
 Figure 1 Comparison between MPC and RSC [6]

output is a function of the preceding inputs and outputs coupled with the projected set of
control signals. This set is computed by minimising an objective or cost function which
aims to align the output as closely as possible to the reference [7]. Once the optimisation
has converged, the first input from the set of control signals is sent to the system and the
process repeats allowing for efficient and accurate reference tracking.
The application of MPC in real-time control systems with fast dynamics, such as DDMRs,
has historically been limited by substantial computational demands. However, this limitation
is becoming more manageable due to the development of faster processors, as documented
in recent literature [8]. The process of predicting outputs and optimising control signals
does remain computationally intensive, requiring devices with high processing capabilities
to perform these calculations with adequate speed to comply with the necessary sampling
periods [9]. The feasibility of implementing such advanced control strategies on the VEX
system must therefore consider the availability and capability of suitable processing hard-
ware.

Figure 2 MPC Optimisation Structure [7]

2.3 Application of Model Predictive Control for Differential Drive Robots

A substantial body of literature validates the use of MPC for controlling DDMRs, with
studies often demonstrating significant improvements in tracking. Experiments frequently
show that MPC provides rapid response times and greater accuracy than traditional control-
lers such as PI controllers. However, many of these studies have been limited to simulations

3
without hardware implementation, often overlooking real-world complexities like backlash
or slip and the processor speed requirements [10–12]. A further deficiency within literature
is the shortage of trials on complex, dynamic paths. All consulted sources tested the per-
formance of the controller using singular waypoints or simple trajectories such as circles,
neglecting sharp corners which are likely required for this project.
Another lack within literature is testing on complex dynamic paths. All found sources will
test the validity of the system using singular waypoints or graphical trajectories such as
circles.

2.3.1 Common MPC Models Relevant to DDMRs

The most commonly used MPC models either simulated or implemented on DDMRs in
literature are outlined below:

• Generalised Predictive Control (GPC): utilises non-parametric models such as step or

impulses

• Linear MPC (LMPC): requires a linear parametric model and quadratic objective
function

• Non-Linear MPC (NMPC): allows the use of a non-linear plant model and uses a
quadratic objective function

Comparisons between GPC and LMPC show that while GPC can offer increased flexibility
and robustness in control, when tested on a DDMR both MPC models exhibit similar levels
of accuracy as shown in Figure 3 where OMPC refers to the linear model. Differences
show in the control input and computation efficiency where LMPC outperforms GPC with
a more fluid response and significantly lower computational time [13].

Figure 3 MPC Model Comparison [13]

As the DDMR is a non-linear system, investigations into the comparison between using
LMPC or NMPC were necessary. There are conflicting results in literature regarding the
optimal approach between LMPC and NMPC, particularly in scenarios requiring real-time
path tracking. It is however clear that NMPC offers superior accuracy and adaptability to
rapid path changes making the system more robust compared to LMPC, which could be

4
crucial in navigating complex paths [14, 15]. The use of NMPC can however come at the
cost of diminished real-time performance on account of increased computational demand.
This may result in the model not being suitable, however, adjusting key horizon parameters
to reduce the volume of calculations required can greatly improve the computation time [16].
2.3.2 Software Tools for MPC

For practical implementation, the selection of appropriate software tools is vital. The
Control Toolbox (CT), an open-source C++ library designed for modelling and trajectory
optimisation, stands out due to its comprehensive resources and compatibility with various
simulation environments. It’s well-suited for real-world applications, facilitating the trans-
ition from simulation to actual robot control [17]. Additionally, MATLAB’s MPC toolbox
is a strong candidate for use attributed to its familiar environment, offering straightforward
implementation and simulation capabilities within Simulink. It also contains the option to
automatically convert functions into C++ which may benefit hardware implementation [13].
Also discovered was the CasADi library, while this is an optimisation library rather than
directly developed for control systems, it has compatibility with a range of languages in-
cluding MATLAB, Python and C++. Furthermore, there are available resources that utilise
this library for MPC implementation giving a baseline to assist in efficient implement-
ation [18]. A comparison between utilising the MPC toolbox and developing with the
CasADi library is shown in Figure 4. This reveals that the latter could run significantly
faster than MPC toolbox with a better level of accuracy [19].

Figure 4 Tracking Error Comparison between MPC toolbox and CasADi [19]

2.4 Literature Review Conclusions

Investigations into available literature have confirmed that implementing MPC for path
tracking in mobile robotics is not only feasible but also capable of delivering highly
accurate and efficient performance. Despite its potential, deploying MPC requires careful
consideration of the system’s operational environment, particularly regarding the processing
delays introduced by computational tasks, sensor inputs, and localisation mechanisms. For
instance, the effectiveness of MPC can be significantly compromised by the delays inherent
in the camera-based localisation system used in the remote lab, which may not affect simpler
control methods like pure pursuit as drastically. Therefore, minimising or compensating for
these delays within the control logic is essential for the successful application of MPC [20].
Moreover, while NMPC appears to offer the best performance in terms of accuracy and

5
responsiveness, it also poses substantial computational challenges. It is crucial to ensure
that the computational demands of NMPC remain within the feasible limits of the VEX
robot’s processing capabilities, especially considering the tight time constraints typical of
dynamic real-time control systems.
Utilising a high-powered, variable library such as CasADi opposed to other available control
toolboxes indicates the potential for enhanced optimisation and faster computation. This is
essential if utilising a more complex MPC model such as NMPC.
These findings guided the subsequent phases of this project, particularly in selecting appro-
priate computational strategies and tools that balance performance with practical limitations.
The insights gained from this literature review helped tailor the MPC implementation to
meet the specific needs of the University of Auckland’s remote lab environment, ensuring
that the final setup enhances both the functionality and educational value of the system.

3. Project Scope and Objectives

This project, set in the academic year of 2024, is dedicated to enhancing the path tracking
capabilities of the differential drive VEX robot within the remote lab at the University of
Auckland. The overarching aim is to develop a highly reliable system that ensures the robot
can successfully navigate to a desired position accurately and promptly. The significance
of achieving this lies in the direct impact on the utility and educational value of remote
labs for students in MECHENG 201, minimising the need for manual adjustments and
maximising the effective use of lab time.
The motivation behind this project is underscored by the existing limitations in the cur-
rent path tracking performance, which presently does not allow for successful orientation
control. This greatly impedes the educational potential of the remote labs, necessitat-
ing significant enhancements to ensure the lab’s efficacy in delivering hands-on learning
experiences remotely.
3.1 Research Objectives

The following research objectives were developed to assist in determining the success of
this project:

• MPC System Exploration and Simulation: To conduct in-depth research and simu-
lations on MPC systems tailoring it for path tracking. This involves assessing the
viability of implementing an MPC system on VEX hardware, focusing on its potential
to improve path accuracy and reliability significantly.

• System Implementation: To implement the designed control system on the VEX

robot and rigorously test in the remote lab environment. This includes not just the
application of non-linear MPC but also the exploration of alternative path tracking
methods if necessary.

• Performance Metrics and Improvement Validation: To systematically evaluate the per-

formance of the implemented path tracking systems using predefined metrics. These
metrics will encompass positional and orientation accuracy, total path completion
time, and overall reliability. A key target is achieving a path completion reliability
rate of 99%, ensuring substantial benefits to students.

6
 • Assessment of Remote Lab Benefits: To quantify the improvements in the remote
lab’s reliability and educational value, potentially incorporating student feedback to
mitigate user bias. This will involve comparing the new system’s performance against
previous year’s data to document progress and identify areas for further improvement.

4. Design Methodology

The methodology adopted for analysing the implementation of MPC for path tracking on a
VEX robot was structured to ensure a comprehensive evaluation and efficient development.
Initially, the project focused on developing the MPC framework and conducting theoretical
simulations. These simulations were crucial for gaining a deeper understanding of the
system’s dynamics, enabling a smoother transition to physical implementation. Additionally,
they served as preliminary tests to assess the potential of MPC in enhancing the robot’s
path tracking capabilities compared to existing methods.
To complete these preliminary tests, a model based on the Pure Pursuit algorithm was also
developed, closely mirroring the actual hardware setup to establish a reliable comparison
baseline. Both the MPC and Pure Pursuit models were simulated in MATLAB on an
equivalent plant model to ensure unbiased assessment. Final testing was planned to be
conducted in the remote lab, where MPC’s performance would ideally be compared against
the Pure Pursuit controller under identical path conditions. Unfortunately, hardware limita-
tions prevented this direct comparison, and alternative evaluation methods are discussed in
Section 7.
To objectively assess the performance and ensure relevance to the remote lab, a set of
critical metrics was defined. These included the accuracy of the robot’s final position
and orientation, the maximum path deviation to prevent collisions, the total time taken to
complete the path, and the controllers reliability. These metrics aimed to optimise both the
accuracy and efficiency of the path tracking, enhancing the educational value of the VEX
robot.
This structured approach not only aligns with the project’s overarching goal of improving
educational tools but also ensures that each phase—from simulation to final testing—directly
contributes to the intended outcomes.

5. Model Predictive Controller Design and Implementation

The architecture of the MPC implemented in this project is shown in Figure 5 which
provides a visual framework for the systems design. The subsequent sections detail the
development process of this framework including the design of the plant model, objective
function, optimisation methods and how it was implemented in hardware. Each component
was designed with the goal of improving path tracking accuracy to enable the success of
this projects aims.

5.1 Robot Plant Model

In order to compute the most optimal solutions, MPC requires a plant model that accurately
represents the system allowing for control of the robots state vector, defined by x =
[x, y, θ]T . The VEX robot used in this project is a rear-wheel differential drive robot,
which is inherently a non-linear system. The decision to trial the NMPC model meant
linearisation was not necessary resulting in two potential options to represent this system.

7
 MPC

Optimiser

Plant
+
-
Plant Model

State Estimator

Figure 5 MPC Controller Block Diagram

The first is a kinematic model, utilising the robots dimensions and first-order kinematic
equations, the second is to construct a second-order kinetic model using the robot’s mass
and inertia. A kinetic model may improve the quality of response allowing the controller
to compensate for momentum and inertia, however, there is more uncertainty in the model.
This is attributed to the experimental methods of determining the mass moment of inertia
along with the assumptions required to simplify the model to a usable state. This combined
with the inevitable increase in computation time consistent with using a more complex plant
model offsets the potential gain, therefore for this implementation, the kinematic model was
used.
The kinematic system of equations, shown in equation (1), uses the coordinate system of the
DDMR schematic in Figure 6 and is the plant model used for both the MPC optimisation
and simulations within MATLAB. The variable u represents the control signal determined
by the controller where u = [v, ω]T . Due to the cyclical nature of angles, adjustments to
the position vector were necessary to handle cases of overflow or underflow, ensuring that
the controller consistently reacted to the shortest rotational path from the target orientation.
This was accomplished by normalising the angular error to ensure it remained between π
and −π, before subtracting the normalised error from the target angle to accurately adjust
the state vector element.
 
u1 cos(θ)
ẋ =  u1 sin(θ)  (1)
u2

The VEX robot uses an inbuilt PID controller for speed control which takes inputs for the
left, ϕ˙L , and right, ϕ˙L , motors in revolutions per minute. The conversion used for this is
shown in equations (2) and (3) where positive rotation is defined in Figure 6.
30(u1 − W u2 )
ϕ˙L = (2)
πR
30(u1 + W u2 )
ϕ˙R = − (3)
πR

5.2 Objective Function and Optimisation

The core of MPC lies in the optimisation of an objective function. This combined with the
plant model defined in section 5.1 is used to generate a series of control signals that will

8
 Figure 6 Schematic of Differential Drive Robot

navigate the robot to the desired coordinates. Therefore, in order to achieve both efficient
and accurate point tracking, the objective function must represent these metrics.
The default objective function implemented in this project was composed of two distinct
terms directly associated with goal metrics. The first term includes the state error, defined
by es = xref − x, where xref is the desired state ensuring position tracking. The second
term includes the control effort u which optimises the control effort allowing the potential
for smoother actuation and tracking. The objective function used for NMPC is a quadratic
function which penalises large deviations in both error and control effort whilst ensuring
a single global minimum enabling reliable optimisation. The objective function shown in
equation (4) includes the weighting matrices Q3×3 and R2×2 which are both diagonal square
matrices with diagonal elements corresponding to the desired response of the respective
variables. The methods for determining the exact values of the weighting matrices are
outlined in section 6.

J = eTs Qes + uT Ru (4)

For optimisation of the objective function, the interior point solver from the CasADi library
was used. This method involves moving from one point on the objective function to
another within the feasible region. It employs a two phase approach: the first phase finds
a feasible solution, and the second phase refines it to optimality. Interior point methods
are generally more powerful and efficient than traditional methods, such as the simplex
algorithm, allowing the MPC to send optimal control signals to the plant [21]. This solver
also allowed for control of maximum iterations and acceptable tolerances combined with
the ability to access optimisation data for efficient debugging. Further MPC setup can be
found in Appendix A.
5.3 Path Tracking Methodology

The inherent purpose of this project is to allow for accurate path tracking beyond that of
a singular waypoint. Accomplishing this task was a multiple step process involving the
construction of a path suitable for the controller, allowing fluid iteration between waypoints
on the path and ensuring path tracking between waypoints. Simulations in MATLAB were
conducted to carry out these steps and identify the most effective method for hardware
implementation. All simulations utilised the plant model defined in section 5.1.

9
5.3.1 Path Construction

The current hardware system for the VEX Remote Lab employs a path planning method
using the A* algorithm to generate a series of [x, y] coordinates for navigating the DDMR
from its starting position to a target location while avoiding obstacles. This algorithm was
designed for the Pure Pursuit control method and relies on a large number of points, with
ARUCO markers used to define obstacles [3, 4]. There are compatibility issues between
the existing path construction and MPC, notably the absence of orientation goals within
the path and the high waypoint proximity which can result in impaired MPC performance
due to consistently small state error values. Additionally, the incorporation of the ARUCO
markers and dynamic path generation is beyond the scope of this project. Therefore, all
paths were predefined and included a large variation of manoeuvrers to thoroughly test the
controllers capabilities.
Waypoint proximity in the generated path required tuning due to the nature of the control
model. As the optimised solution generated by the MPC does not always result in a path
aligning to the desired trajectory, especially when the desired orientation is significantly dif-
ferent to the path gradient, this can result in large path deviation and interesting trajectories.
An example of this is shown in Figure 7 where the position response finds the optimal
tracking solution to be reversing into the second to last point opposed to navigating straight
and turning. Therefore, the proximity of waypoints was determined to be a function of the
difference between the gradient between waypoints and the desired orientation.

1.6

1.4

1.2
y position [m]

0.8

0.6

0.4 Desired Path 1

Robot Path 1
0.2 Desired Path 2
Robot Path 2
0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6
x position [m]
Figure 7 Controller response for differing paths implemented on hardware

5.3.2 Waypoint Iteration Method

The default implementation of MPC optimises control efforts for navigation to only one
waypoint at a time. The method of updating the desired position only after the MPC had
completed the prior one is inefficient, resulting in the robot coming to a near stop at each
point. To address this, two methods were trialled in simulation. The first method involved
projecting waypoints linearly, creating a larger positional error, allowing the controller to
perceive a greater distance to the target and maintain higher speeds. Once the robot
approached a predetermined error threshold from the desired position, the target shifted to

10
the next waypoint. The second method was simpler, involving interrupting tracking and
updating the waypoint early, before the MPC had completed. This kept the state error
larger, helping the robot maintain velocity instead of stopping at each point.
Figure 8 compares the two methods in simulation. Waypoint projection proved more
efficient, completing the path in 24.5 s compared to 27.9 s for the early interrupt method.
While faster, waypoint projection showed a similar level of actuator fluctuation due to
the robot reaching points quicker but having to overcompensate by stopping and turning
abruptly or reversing at sharp corners reducing path accuracy. This is shown in Figure 8b
with the larger, more frequent periods of negative linear velocity. As a result, the early
interrupt method was chosen for its greater overall accuracy and fluidity.

Linear Speed [m/s]

0.6

1.6 0.4

1.4 0.2

1.2 0
y position [m]

1 -0.2
0 50 100 150 200 250 300

0.8
Angular Speed [rad/s]
0.5

0.6

0.4
0
Desired Path
0.2 Early Interrupt Early Interrupt
Waypoint Projection Waypoint Projection
0 -0.5
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 0 50 100 150 200 250 300

x position [m] timestep [s]

(a) Positional Result (b) Control Effort

Figure 8 Comparison of path tracking methodologies for MPC

5.3.3 Intermediate Waypoint Path Tracking

Further enhancements were made to optimise the MPC controller for tracking the path
trajectory beyond individual waypoints. The base nature of MPC aims to compute the
most efficient control signal to reach its goal location, which does not always correspond
to the desired trajectory between points. The solution to this involved incorporating an
additional term to the MPC’s objective function to minimise the distance between the
robot’s instantaneous position and the ideal location along the path at each corresponding
moment. This concept was implemented by representing the path between waypoints as
a linear equation and identifying the coordinates on this line closest to the robots current
position. This value is denoted as d in the updated objective function in equation (5), where
P represents the path weighting for efficient tuning of the controller. The effectiveness of
this addition is demonstrated in section 6.

J = eTs Qes + uT Ru + d2 P (5)

5.4 Hardware Implementation

The desired environment for hardware implementation is the VEX remote laboratory. This
environment was developed in 2023 at the University of Auckland with the system shown in
Figure 9 incorporating the VEX V5 core, USB web camera and a 4th generation Raspberry
Pi. System updates to the University of Auckland’s internet security rendered the Wi-Fi
connection unusable for remote access to the Raspberry Pi via VNC Viewer. Fixing this

11
issue was out of the project scope therefore a wired connection was used to allow for
communication between the positional data obtained by the image processing algorithms
and the robots EKF.

Robot Lab Computer

USB Web VEX Lab Computer Raw

User Interface Position Data
Camera
USB Corrected
State Estimator Position Data
V5 Cortex Camera Data
User Interface
Control Effort

USB Controller
Raspberry Pi 4 WiFi
VNC Viewer Open CV Camera
Actuators

(a) Communication Channels (b) Data Streams

Figure 9 Original Hardware Setup

Implementing the controller necessitated the use of an optimisation library compatible with
the PROS API used for programming the VEX V5 Cortex. Although various libraries sup-
port C++, the language utilised by the PROS API, operational system and time constraints
prevented the successful building and execution of any of these libraries on the VEX. Lib-
raries tested included CasADi, libmpc, and NLopt [22, 23]. Initially, the Raspberry Pi was
intended solely for communication between the VEX and the camera system. However,
since this component was no longer utilised, it was repurposed to run the MPC algorithms
using Python, which is compatible with CasADi. This helped facilitate efficient transfer
from of the controller from MATLAB to Python.
The final hardware implementation is described as follows: The graphical user interface
(GUI) developed in the 2023 project was employed to transmit positional data, desired
locations, and command signals to the Raspberry Pi via a USB to TTL converter. This
setup enabled the Raspberry Pi to function as a device rather than a host, allowing for data
transmission between the two computers. A Python script containing the MPC controller
was developed on the Raspberry Pi, comprising three distinct threads; main, VEX and GUI.
The main thread executes the finite state machine, which alternates based on commands
received from the GUI, including ’stop’, ’manual’, and ’position’. Each command computes
the corresponding motor power required for the robot and updates a buffer for VEX data
transmission. Positional data is sent from the GUI only after a path is selected and initiated;
thus, the ’position’ state triggers the MPC function to commence path tracking.
The other two threads manage the GUI and VEX communication channels, continuously
polling for serial requests. The GUI thread receives and parses incoming messages based
on the initial character. When raw position data from the camera is received, it is directly
forwarded to the VEX thread. The VEX thread then sends this raw positional data to its
state estimator utilising an EKF and receives the corrected state estimate, enabling the MPC
to compute optimal control signals accurately. Additionally, this thread sends the desired
motor speeds defined by the buffer updated in the main thread.
. The Raspberry Pi communicates with the VEX using a serial protocol, allowing the
VEX to parse data for either the EKF or the motors based on the command identifier. An
overview of this hardware setup is presented in Figure 10, with the relevant script outlined
in Appendix A.

12
 VEX Raspberry Pi Lab Computer Raw
Position Data
Corrected
State Estimator Main Thread Position Data
Camera Data
User Interface
Control Effort

Main Controller VEX Thread GUI Commands

Open CV Camera
Actuators GUI Thread

Figure 10 Revised Hardware Setup

6. Parameter Optimisation

All preliminary tuning was accomplished using MATLAB simulations including the obser-
vation of different parameters and their impact on the controller response. The approximate
ratios of the tuned simulation were then implemented on the hardware and further tested to
obtain the optimal control signals for path tracking of the DDMR. Parameters to be optim-
ised included the state, path and controller weights used within the objective function and
the horizon length. All parameters are optimised on the basis of improving the predefined
critical metrics.

6.1 Horizon Length

The horizon length for an MPC controller is composed of a prediction and control horizon.
The prediction horizon, Np , defines how far into the future system states are predicted
with the set of future control inputs, while the control horizon, Nc , defines the number of
time steps within the prediction horizon for which the control inputs are actively optimised.
Larger prediction horizons lead to increased foresight and therefore more accurate long-term
behaviour but also increases the computational complexity of optimisation. This leads to
longer computation time. A control horizon equal to prediction horizon means that every
future control signal, used to predict the state at each concurrent time instant, within the
prediction horizon is optimised and is the method used for this MPC implementation. The
horizon length is therefore denoted by N such that N = Nc = Np . In future attempts,
alternating the control horizon would ideally also be trialled but this was not done for this
project.
Simulations testing the impact of changing horizon length are shown in Figure 11 with the
quantified performance metrics outlined in Table 1. The test for N = 20 resulted in path
failure representing how dependent the response is on the controllers foresight. When the
horizon length is small it cannot fully anticipate long-term effects of the control effort on
the system and will not react accordingly. Beyond the failure of N = 20, all other horizon
lengths showed increasing accuracy and speed as is to be expected. The impacts of these
improvements do eventually plateau showing that a horizon length of N = 40 is sufficient
to generate optimal control efforts for path tracking in simulation.
The horizon length of N = 40 was used as the initial value for hardware implementation
before trialling and observing the results of other potential values. The main difference
between simulation and hardware in regards to horizon length lies in the computational time
taken for optimisation. The MPC works off a predicted time step of 0.1 s, therefore, if
the desired control signal is not sent to the robots actuators at this approximate frequency

13
 Table 1 Comparison of Horizon Length Metrics in Simulation

Metrics N = 20 N = 30 N = 40 N = 60
Final Distance Error [mm] 479.0 7.43 4.41 2.52
Final Angle Error [º] 1.57 1.76 1.50 1.36
Maximum Distance from Path [mm] 115.6 55.8 47.3 45.0
Total Time Taken [s] 42.6 28.6 28.3 27.8

Linear Speed [m/s]

0.6

1.6
0.4

1.4
0.2
1.2
y position [m]

0
1
0 50 100 150 200 250 300 350 400 450

0.8

Angular Speed [rad/s]

0.5

0.6
Desired Path
0.4 N20 0 N20
N30 N30
0.2 N40 N40
N60 N60
0 -0.5
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 0 50 100 150 200 250 300 350 400 450

x position [m] timestep [s]

(a) Position Response (b) Control Effort

Figure 11 Simulated Horizon Length Comparisons

the accuracy of tracking degrades proportionally to the lag. As shown in both Table 2
and Figure 12 while N = 20 does not fail with the hardware tuning configuration, it is
still insufficient to accurately and efficiently path track as found in simulation. Differences
in the nature of the positional response between simulation and hardware implementation
occur once the horizon length increases beyond N = 30. All performance metrics show
performance degradation as a result of the increased computation time demonstrated by
Sub-Figure 12b which shows the average time in seconds between MPC iterations for each
horizon length. As a result of this analysis, a horizon length of 30 was chosen for the final
tuned system implemented on hardware.
Table 2 Comparison of Horizon Length Metrics in Hardware

Metrics N = 20 N = 30 N = 40 N = 60
Final Distance Error [mm] 31.4 4.4 21.3 18.2
Final Angle Error [º] 0.1 1.0 2.3 3
Maximum Distance from Path [mm] 79.8 46.6 101.5 132.6
Total Time Taken [s] 46.8 39.8 44.9 57.7

6.2 State Weighting

The diagonal elements of the Q matrix in the objective function heavily impact the per-
formance of the controller due to their direct relationship with how the control signal is
optimised. Each element corresponds to a state variable, x, y or θ, and an increase in value

14
 1.6 350

1.4 300

Average Delta Time [ms]

1.2
250
y position [m]

1
200
0.8
150
0.6
Desired Path 100
0.4 N = 20
N = 30 50
0.2 N = 40
N = 60
0
0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 20 30 40 60
x position [m] Horizon Length (N)
(a) Positional Response (b) Average Time Step

Figure 12 Hardware Horizon Length Comparisons

will increase how much that variable is optimised in the response. Figure 13 shows the
results of alternating the weightings of these elements. In simulation, for a large horizon
length there is minimal impact on the path tracking accuracy when adjusting the Q mat-
rix due to the controllers ability to account for the robots long-term dynamics. However,
for lower horizon lengths, any change in weighting such that the θ element has a higher
weighting than other state variables will result in failure as demonstrated in Figure 13b.
This is due to the robots dynamics such that if orientation tracking is prioritised over
position, for any goal orientation significantly different to the path gradient, the controller
will not be able to find a feasible solution. This is because it cannot both drive toward the
waypoint whilst maintaining a significantly different orientation. This behaviour analysis
was essential in developing the weightings for the hardware implementation due to the
lower horizon length required. The best tracking results were generated using similar x
and y weightings with a θ weighting of approximately half those values.

1.6 1.6

1.4 1.4

1.2 1.2
y position [m]

y position [m]

1 1

0.8 0.8

0.6 0.6
Desired Path Desired Path
0.4 Equal Weighting 0.4 Equal Weighting
Low Theta Weighting Low Theta Weighting
0.2 Low X Weighting 0.2 Low X Weighting
Low Y Weighting Low Y Weighting
0 0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6
x position [m] x position [m]
(a) High Horizon Length (b) Low Horizon Length

Figure 13 Simulation State Weighting Comparisons

6.3 Control Weighting

Much like that of the state weighting matrix, Q, the diagonal elements of the control
weighting matrix, R, correspond to the optimisation of the linear and angular control

15
inputs. Imbalance between the two weightings will change the method of tracking such
that increasing the weight of either relative to the other will optimise the control input to
allow for smoother signals in the input of the higher value. This can be clearly shown in
Figure 14b where for the decreased linear weighting, the linear control signal goes negative
when turning due to the controller prioritising smooth angular velocity. This results in the
robot tracking similar to that of a car, utilising reverse to navigate turns through a three
point turn opposed to turning on the spot as the robot is capable of. This results in jerky
path tracking that takes significantly longer, a total time of 37.6 s, than the case with a
lower angular control weighting, a total time of 25.7 s. Equal control input weightings
result in slightly better tracking with a final distance error of only 5 mm compared with the
10 mm of both alternate options, but a longer total time taken of 38.9 seconds. Due to these
factors an optimal ratio was acquired between the two weightings such that the angular
weighting was 20% lower than the linear control weighting for the fully tuned simulated
model. This weighting was used as the basis for the tuning the hardware implementations
control weightings.

Linear Speed [m/s]

0.6

1.6 0.4

1.4 0.2

1.2 0
y position [m]

1 -0.2
0 50 100 150 200 250 300 350 400

0.8
Angular Speed [rad/s]

0.5

0.6

0.4 Desired Path 0

Equal Weighting Equal Weighting
0.2 Low Angular Control Weighting Low Angular Control Weighting
Low Linear Control Weighting Low Linear Control Weighting
0 -0.5
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 0 50 100 150 200 250 300 350 400

x position [m] timestep [s]

(a) Positional Response (b) Control Effort

Figure 14 Simulation Control Weighting Comparisons

The hardware implementation showed an exaggerated response of the behaviours found in

the simulated control weightings response. The benefits of a lower angular control effort
were more significant offering both large increases in tracking accuracy and the total time
taken to complete the path. These exact values are outlined in Table 3 demonstrating the
efficacy of prioritising smooth linear motion whilst allowing for sharp turning. The reason
behind this can be connected to the differences between the simulated model and the real
model, where impacts such as inertia and friction play a larger impact. The final ratio of
control weightings was then determined to be an angular control weighting equivalent to
50% of the linear control weighting.
6.4 Path Weighting

As specified in sub-section 5.3.3, an additional term was included in the objective function
allowing the controller to optimise for trajectory tracking between waypoints. The efficacy
of this solution was thoroughly tested in simulations trialling a range of P weightings. As
shown in Figure 15, the addition of this term makes a significant difference even with a
low weighting and continues to show improvement with higher weightings in simulation.
The only negative for this addition is the increase in time taken to complete the path which
is proportional to the weighting. The high path weighting took approximately 6 seconds

16
 Table 3 Comparison of Control Weighting Metrics in Hardware

Metrics Equal Low Angular Low Linear

Weighting Control Control
Weighting Weighting
Final Distance Error [mm] 27.7 13.4 17.1
Final Angle Error [º] 1.3 0.4 1.9
Maximum Distance from Path [mm] 78.6 58.2 88.4
Total Time Taken [s] 36.5 29.9 36.3

longer to complete the path than the low path weighting. As there was only a small
increase in the time metric for the low path weighting relative to the absence of the term,
the lower weighting was chosen along with the decision to keep the term for hardware
implementation.

1.6

1.4

1.2
y position [m]

0.8

0.6

0.4 Desired Path

High Path Weighting
0.2 Low Path Weighting
No Path Weighting
0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6
x position [m]

Figure 15 Simulated Path Weighting Comparisons

Implementing this method in hardware still accomplished the desired effect, just not to the
extent of the simulated response. The results of comparing a range of path weightings is
shown in Figure 16. Whilst there is definitely an improvement between utilising the path
error term and not, there is minimal observable difference in adjusting the weighting. It
is only upon calculating the critical metrics a difference can be seen. These metrics are
shown in Table 4 and reveal that the lower path weighting is actually more effective at
improving the tracking accuracy of the robot. This is likely due to the overall relationship
between terms of the objective function, such that increasing the path weighting too much
may result in less optimisation of the states themselves and actually degrade the controllers
tracking performance.

7. Pure Pursuit

The aims of this project require a comparison against Pure Pursuit to effectively validate
the improvement of the designed controller. For initial comparison in simulation, a pure
pursuit controller using the same methods as implemented on hardware was constructed

17
 Table 4 Comparison of Path Weighting Metrics in Hardware

Metrics Equal Low Path High Path

Weighting Weighting Weighting
Final Distance Error [mm] 10.8 3.7 24.8
Final Angle Error [º] 1.2 1.0 0.8
Maximum Distance from Path [mm] 77.0 50.1 60.0
Total Time Taken [s] 28.1 29.9 36.8

1.6

1.4

1.2
y position [m]

0.8

0.6

0.4 Desired Path

High Path Weighting
0.2 Low Path Weighting
No Path Weighting
0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6
x position [m]

Figure 16 Hardware Implemented Path Weighting Comparisons

and trialled on the same plant model as the MPC.

Pure Pursuit works by identifying a target point on the path within a predetermined looka-
head distance, l, and calculating a curvature variable, γ, based on the angle the robot
should turn to reach this point, α, and the distance from the point, d. The formula for
the curvature calculation is shown in equation (6). The curvature is directly proportional
to the angular velocity applied to the robot by adjusting the speed ratio between left and
right wheels. The robot maintains a constant linear velocity with only change made to the
angular velocity.
2sin(α)
γ= (6)
d
Due to issues with re-implementing Pure Pursuit on the VEX hardware with the changes
made to the system, an attempt at computing the predicted hardware results for pure pursuit
were found by extrapolating the mean speed over the constructed path distance to predict
total time taken. Both the final error and mean path deviation for pure pursuit was noted
in [3, 4] and used for comparison. As there was no maximum path deviation found, the
mean path deviation was used as an alternate method of comparison. There was no final
orientation error comparison due to pure pursuits inability to control the final orientation
of the robot.

18
8. Results

This section presents the results from both simulation and hardware trials conducted to
evaluate the performance of the MPC system implemented on the differential drive VEX
robot. Comparisons using the above stated methodology of collecting Pure Pursuit data
are shown in the subsequent figures and tables. The following results trial the control
methodologies on a complex path designed to test both smooth and sharp turns ensuring
robustness. It also incorporated a desired final orientation of 0° to determine the controllers
possibility of accurately tracking a final angle accurately that differs from the path trajectory.
8.1 Simulation

Simulations were conducted using MATLAB to model the robot’s response under MPC
and Pure Pursuit control strategies. Utilising the tuning strategies and impacts of adjusting
parameters described in section 6, the final tuned simulation of the MPC controller is shown
in Figure 17. The controller works most effectively around the initial, smoother curve hitting
all waypoints effectively whilst the performance degrades slightly when encountering sharper
turns. This is likely due to the contradictions between optimising for the final orientation
compared with the coordinate position meaning for the controller to match the path exactly
it would need to drive directly to a point before turning. This is however not something
necessarily desired as it is a less efficient method of path tracking that does not effectively
utilise the benefits of MPC. Therefore, the minor deviations from the desired path actually
result in a more efficient tracking method which is something desired by our project so
long as the maximum deviation is not too large.

1.6
Start
1.4

1.2
y position [m]

0.8

0.6

0.4
End Desired Path
0.2 Robot Path
Final Orientation
0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6
x position [m]
Figure 17 Tuned Simulated MPC Path Tracking

The comparisons between the Pure Pursuit controller and MPC are shown in Figure 18.
The pure pursuit controller has a smoother path following technique due to its intrinsic
design of computing wheel speeds based on the path curvature however it has a tendency
to cut corners as shown in sub-figure 18a. It also had to maintain a lower linear speed
than the MPC shown in Figure 18b, in order to complete the path as the model is based

19
off constant linear speed model. This meant that when trialled at higher velocities, the pure
pursuit controller would overshoot leading to the lower default linear speed. This lack of
speed variation means that for pure pursuit to have comparable accuracy results to MPC,
it comes at the cost of efficiency and vice versa. Table 5 represents the quantified metrics
for comparing the controllers and demonstrates that in simulations the MPC outperforms
pure pursuit in all metrics with a huge improvement in the final error, an 88.57% decrease,
and further improvements in all other metrics.

Table 5 Simulated Final Results Comparison

Final Final Angle Maximum Total Time

Distance Error [º] Distance Taken [s]
Error [mm] from Path
[mm]
Model Predictive Control 2.52 1.36 45.0 27.8
Pure Pursuit 22.4 N/A 53.7 32.2
Improvement 88.6%_ N/A 16.2% _ 13.7% _

1.6
Linear Speed [m/s]

0.6
1.4
0.4
1.2
y position [m]

0.2
1
0

0.8 0 50 100 150 200 250 300 350

Angular Speed [rad/s]

0.6 0.5

0.4
Desired Path 0
0.2 MPC
Pure Pursuit MPC
Pure Pursuit
0
-0.5
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 0 50 100 150 200 250 300 350

x position [m] timestep [s]

(a) Position Response (b) Control Effort

Figure 18 Simulated Controller Comparison

8.2 Hardware

The positional data used in hardware comparisons came from the robots EKF utilising
the camera position tracking. This system showed accuracy of less than one millimetre
and was the same system used for the implementation of pure pursuit, therefore, for the
purposes of this project, the position data was assumed accurate. Hardware testing of
the MPC revealed comparable results to the simulations with the best tracking occurring
on the smoother turns. The controller still reached every waypoint on the path with
relative accuracy however the tracking was considerably less fluid shown by the 3-point
turn technique on the sharp corners in Figure 19. This again is something that could be
improved by adjusting weightings to prioritise smoothness at the cost of accuracy. The
final orientation error was also extremely good with a mean error of 1.3°.

20
Determining the consistency of results was also essential and showed reliable tracking of
the MPC on hardware. There was a 100% success rate for repeatability trials, with only
failures occurring when the GUI itself crashed preventing position data transmission. Fixing
this is out of the scope of this project therefore reliability tests were only conducted on
the controller implementation itself which included the Raspberry Pi to VEX connection
which worked consistently without failure. There were some deviations in accuracy with
a final error standard deviation of 7.1 mm as shown in Table 6. While this is relatively
large compared to the mean error of 11.3 mm, when in context of the remote lab, which
includes a 1.6 m x 1.6 m course size, a final error of approximately 1 cm and even smaller
deviation is minute. There was also very minimal deviation in the total time taken which
is ideal as it implies tracking will be consistent and efficient for each use.

Table 6 Hardware Final Repeat Results

Run Run Run Run Run Mean Std

1 2 3 4 5 Devi-
ation
Final Distance Error [mm] 11.6 4.4 16.8 11.6 23.6 11.3 7.10
Final Angle Error [º] 1.0 1.0 1.8 1.0 1.5 1.3 0.37
Maximum Path Deviation [mm] 43.9 46.6 47.0 62.0 53.0 50.5 7.23
Mean Path Deviation [mm] 11.9 15.5 16.9 20.6 15.2 16.0 3.12
Total Time Taken [s] 40.7 39.8 41.3 38.0 41.0 40.2 1.33

1.6 1.6
Start
1.4 1.4

1.2 1.2
y position [m]

y position [m]

1 1

0.8 0.8

0.6 0.6 Desired Path

Run 1
0.4 0.4 Run 2
End Desired Path Run 3
0.2 Robot Path 0.2 Run 4
Final Orientation Run 5
0 0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6
x position [m] x position [m]
(a) Best Controller Response (b) Repeatability

Figure 19 Tuned Hardware MPC Path Tracking

Accurate comparisons against the Pure Pursuit controller proved difficult as specified in
section 7, therefore these results are a preliminary indication of success requiring further
validation. The extrapolation of the Pure Pursuit results generated by the 2023 implement-
ation are shown in Table 6 [3, 4]. A slight shift to comparing the mean path deviation was
used as an alternative to the maximum path deviation as that was the data available. The
comparison of final distance error is less related to path construction and is therefore the
most accurate comparison. This result showed, similarly to the simulations, that the final
distance error of the MPC implementation was approximately 42% less than that of pure
pursuit lending good evidence to an improvement in path tracking accuracy. The mean path

21
deviation also shows improvement, however, the speed at which the MPC completes the
path is longer than the predicted pure pursuit time. This is to be expected as both hardware
implementations had equivalent speed limitations. Where MPC slowed to improve accur-
acy, the pure pursuit controller would maintain the high linear speed allowing for faster
completion.

Table 7 Simulated Final Results Comparison

Mean Final Mean Final Mean Path Mean Total

Distance Angle Error Deviation Time Taken
Error [mm] [º] [mm] [s]
Model Predictive Control 11.3 1.3 16.0 40.2
Pure Pursuit 19.42 N/A 34.12 27.3
Improvement 41.8% _ N/A 53.1% _ 47.3% ^

9. Discussion

Overall, this project was successful in implementing a Model Predictive Controller on a

VEX robot specialised for path tracking. All research objectives were met from design
to hardware implementation and performance validation on both simulated and hardware
against the prior implementation of pure pursuit. The final controller showed improved
tracking capabilities and potential to benefit the experience of students utilising the remote
lab.
Within the literature used to educate the implementation of this project, many of the findings
were consistent. The implementation of MPC in [2] was aligned with this projects findings
resulting in an overall accuracy and robustness greater than that of pure pursuit. Prior tests
conducted in [3, 4] revealed pure pursuit performed consistently better than a PI control
system, therefore it can be assumed that MPC would accomplish the same. There were no
issues found in regards to the instability of the system as briefly mentioned in [5] when
comparing the RSC and MPC systems, implying the implementation is highly relevant and
successful for the controlled environment of the remote lab. This also implies the potential
of better performance than that of a FSFB system based on the literature alone.
The implementation of NMPC was also successful on the dynamic system even with the
increased computation time which was a concern raised in [16]. The impacts of this concern
could be observed when increasing the horizon length, however, with the use of the CasADi
library implemented on a Raspberry Pi this system was sufficient to allow for the robust,
high accuracy tracking consistent with NMPC models [14,15]. This is likely a combination
of using the more efficient optimisation methods of the CasADi library revealed in [19]
in conjunction with the Raspberry Pi’s operating system. Utilising the Raspberry Pi, while
essential because of the VEX hardware’s library constraints, also gave the added benefits of
incorporating a significantly more powerful and efficient processor due to its higher clock
speed and multiple cores. It remains to be seen whether this system could be implemented
on the VEX hardware itself if a compatible library was discovered.
This project incorporated further adjustments to the controller beyond that found in liter-
ature. Most goal paths consisted of singular points or consistent trajectories that did not
require dynamic changes to the goal path. The implementation used in this project exceeded

22
those experimentations by expanding on the objective function to allow for tracking of both
a dynamic path, and less path deviation in between points. This was found to be beneficial
for the aims of this project in minimising the maximum distance from the path, while
recognised as not necessarily a goal for all tracking applications. Depending on system
requirements this could either be a promising addition to improve the accuracy tracking of
other applications or result in degradation in terms of controller speed and smoothness.
In regards to the literature found, this project has met if not exceeded expectations in the
development of a robust and effective path tracking controller. It shows great potential for
further evolution of utilising MPC for path tracking with a highly variable setup allowing
for performance alterations based on the requirements of the system.

10. Conclusions

This aims of this project, outlined in section 3, were all successfully met to varying
standards. The evolution of the remote labs path tracking capabilities has resulted in both
an improved path tracking system but also a significantly different hardware setup.
The exploration into utilising Model Predictive Control for path tracking was indeed suc-
cessful and showed good performance in regards to all critical performance metrics. The
analysis conducted in both research and simulation demonstrated high accuracy of position
tracking and allowed control over final orientation. Both of these metrics were critical in
ensuring this system would be beneficial in improving the students experience within the
remote lab ensuring reliable “go-to” functionality. The improvements to the basic MPC
framework to optimise it for path tracking also showed significant improvement in ensuring
minimal path deviation such that the incorporation of obstacles could be implemented and
would not impact the tracking abilities of the controller. The system also showed potential
in its ability to accelerate over straight distances and slow for turns to ensure accuracy
whilst maintaining high efficiency.
The hardware implemented controller also showed similarly effective results with its high
accuracy and reliability. An evaluation of the critical metrics showed great performance
in final position and orientation error with minimal path deviation. The efficiency of
this system was moderate but not ideal due to the system being highly optimised for
accuracy. The efficiency is good for simple path trajectories which will be beneficial for
the majority of test cases in student labs. Incorporating large amounts of obstacles, resulting
in more complex paths, will greatly increase the time taken for path tracking which while
not commonly utilised in remote labs, may impact the experience for cases when this is
required.
The evaluation of the final system in comparison to prior tracking implementation showed
this objective was successful. The overall accuracy was significantly better in both simu-
lation and hardware than Pure Pursuit. The further addition of orientation control is also
highly valued in the improvement of the system considering Pure Pursuit had no capacity
for this. While the comparison indicated degradation in efficiency, the added accuracy
benefits outweigh this. The further achievement of controller reliability greater than 99%
is ideal in ensuring a robust system that will greatly benefit student experience.
Assessment of the remote labs benefits show that while the controller itself was greatly
improved and outperformed prior implementation, there is one project aspect that falls
short. The implementation into the VEX hardware system proved difficult and while it
was accomplished it resulted in significant changes to the setup. Beyond the hardware

23
limitations in reference to the inability to remotely access the robot, the controller design
would not be compatible with the previous remote lab system. Therefore, for this system
to be fully utilised in the remote lab further changes must be made to the system which
would include constructing a method for remote student code uploads to the VEX robot
to enable student code testing. Previously, this was accomplished using the Raspberry Pi
to upload code however with the system being utilised for tracking this would have to be
accounted for and enabled using an alternate method.
Overall this project concluded with the development of an accurate and robust Model
Predictive Controller optimised for path tracking. This system added value in the addition
of orientation control and improved accuracy validated by comparing both simulations
and hardware against the Pure Pursuit system. Given the projects scope and hardware
constraints this system is not currently capable of being remotely operated, therefore, this
area is suggested as a goal for further research and iteration to expand the benefits of this
system.

11. Future Work

Beyond the necessary improvements to the hardware setup, a suggestion for future work and
research is the potential of utilising the path planning capabilities of MPC. The controller
has the added benefits of allowing constraints within the system. This includes both state
and control variables. Currently, the only constraints being utilised are limits on the control
effort and the remote lab dimensions to avoid the robot going out of bounds. Underutilised
is the possibility of adding constraints that could represent obstacles allowing the controller
to determine the optimal path itself. Brief exploration was done into this area and is shown
in Figure 20 where the goal orientation was opposite to the starting orientation such that the
optimal path found involved reversing into the final position. This could be a unique way
of path planning and shows potential in developing a more efficient method of navigation
if optimised successfully.

Figure 20 MPC Path Planned Response

24
References

[1] Y.-C. Sin, C.-M. Rim, C.-G. Yun, Y.-N. Kim, and K.-P. Choe, “Trajectory tracking con-
trol of differential drive mobile robots using neural network disturbance compensator,”
2024.

[2] Q. Wang, J. He, C. Lu, C. Wang, H. Lin, H. Yang, H. Li, and Z. Wu, “Modelling
and control methods in path tracking control for autonomous agricultural vehicles: A
review of state of the art and challenges,” Applied Sciences, vol. 13, no. 12, p. 7155,
2023.

[3] G. Reddish, “Localisation and position control for a mobile robot remote lab,” prior
Years UoA Report.

[4] J. Kim, “Localisation and position control for a mobile robot remote lab,” prior Years
UoA Report.

[5] M. Auzan, R. M. Hujja, M. R. Fuadin, and D. Lelono, “Path tracking and position
control of nonholonomic differential drive wheeled mobile robot,” Jurnal Ilmiah Teknik
Elektro Komputer dan Informatika, vol. 7, no. 3, pp. 368–379, 2021.

[6] K. Yang, X. Tang, Y. Qin, Y. Huang, H. Wang, and H. Pu, “Comparative study of
trajectory tracking control for automated vehicles via model predictive control and
robust h-infinity state feedback control,” Chinese Journal of Mechanical Engineering,
vol. 34, pp. 1–14, 2021.

[7] P. Dr Eduardo F. Camacho, Model Predictive Control, 3rd ed., ser. Advanced textbooks
in control and signal processing, P. Dr Carlos Bordons, Ed. London: Springer-Verlag
London Limited 1999, 1998.

[8] F. Kuhne, W. F. Lages, and J. G. da Silva Jr, “Model predictive control of a mobile
robot using linearization,” in Proceedings of mechatronics and robotics, vol. 4, no. 4.
Citeseer, 2004, pp. 525–530.

[9] F. Künhe, J. Gomes, and W. Fetter, “Mobile robot trajectory tracking using model
predictive control,” in II IEEE latin-american robotics symposium, vol. 51, 2005, p. 5.

[10] S. Bouzoualegh, E.-H. Guechi, and R. Kelaiaia, “Model predictive control of a

differential-drive mobile robot,” Acta Universitatis Sapientiae, Electrical and Mech-
anical Engineering, vol. 10, no. 1, pp. 20–41, 2018.

[11] M. Cannon and B. Kouvaritakis, “Continuous-time predictive control of constrained

linear systems,” in Theory and Practice of Control and Systems. World Scientific,
1998, pp. 507–513.

[12] E. Kim, J. Kim, and M. Sunwoo, “Model predictive control strategy for smooth
path tracking of autonomous vehicles with steering actuator dynamics,” International
Journal of Automotive Technology, vol. 15, pp. 1155–1164, 2014.

[13] H. N. Huynh, O. Verlinden, and A. Vande Wouwer, “Comparative application of

model predictive control strategies to a wheeled mobile robot,” Journal of Intelligent
& Robotic Systems, vol. 87, no. 1, pp. 81–95, 2017.

25
[14] E. Kayacan, W. Saeys, H. Ramon, C. Belta, and J. M. Peschel, “Experimental val-
idation of linear and nonlinear mpc on an articulated unmanned ground vehicle,”
IEEE/ASME Transactions on Mechatronics, vol. 23, no. 5, pp. 2023–2030, 2018.

[15] B. Guoxing, L. Li, M. Yu, L. Siyan, L. Li, and L. Weidong, “Real-time path tracking
of mobile robot based on nonlinear model predictive control,” Nongye Jixie Xue-
bao/Transactions of the Chinese Society of Agricultural Machinery, vol. 51, no. 9,
2020.

[16] T. P. Nascimento, C. E. T. Dórea, and L. M. G. Gonçalves, “Nonlinear model pre-

dictive control for trajectory tracking of nonholonomic mobile robots: A modified
approach,” International Journal of Advanced Robotic Systems, vol. 15, no. 1, p.
1729881418760461, 2018.

[17] M. Giftthaler, M. Neunert, M. Stäuble, and J. Buchli, “The control toolbox, an open-
source c++ library for robotics, optimal and model predictive control,” in 2018 IEEE
International Conference on Simulation, Modeling, and Programming for Autonomous
Robots (SIMPAR). IEEE, 2018, pp. 123–129.

[18] W. Benchouche, R. Mellah, and M. S. Bennouna, “Navigation of a differential drive

mobile robot using nonlinear model predictive control,” in The 2nd International
Conference on Complex Systems and their Applications, 2021, pp. 188–201.

[19] M. Elhesasy, T. N. Dief, M. Atallah, M. Okasha, M. M. Kamra, S. Yoshida,

and M. A. Rushdi, “Non-linear model predictive control using casadi package for
trajectory tracking of quadrotor,” Energies, vol. 16, no. 5, 2023. [Online]. Available:
https://www.mdpi.com/1996-1073/16/5/2143

[20] G. Klančar and I. Škrjanc, “Tracking-error model-based predictive control for mobile
robots in real time,” Robotics and autonomous systems, vol. 55, no. 6, pp. 460–469,
2007.

[21] F. A. Potra and S. J. Wright, “Interior-point methods,” Journal of computational and

applied mathematics, vol. 124, no. 1-2, pp. 281–302, 2000.

[22] N. Piccinelli, “Libmpc,” 2023.

[23] S. G. Johnson, “Nlopt: Nonlinear optimization library,” Astrophysics Source Code

Library, 2021.

26
 Appendix A Appendix A: Python Script for MPC Class

Program A1 MPC Function and Setup

1 class MPC_class :
2
3
4 max_iterations = mpc_it
5 T = timestep # Sampling time
6 N = horizon # Prediction Horizon
7
8 # Set limits on velocity
9 v_max = minVel
10 v_min = - v_max
11
12 # Set limits on angular velocity
13 omega_max = minAng
14 omega_min = - omega_max
15
16 # Parameters
17 x = ca . SX . sym ( ’x ’)
18 y = ca . SX . sym ( ’y ’)
19 theta = ca . SX . sym ( ’ theta ’)
20
21 # States
22 states = ca . vertcat (x , y , theta )
23 n_states = states . size () [0]
24
25 # Controls
26 v = ca . SX . sym ( ’v ’)
27 omega = ca . SX . sym ( ’ omega ’)
28 controls = ca . vertcat (v , omega )
29 n_controls = controls . size () [0]
30
31 # RHS system - yields vx , vy , omega
32 rhs = ca . vertcat ( v * ca . cos ( theta ) , v * ca . sin ( theta ) , omega )
33
34 f = ca . Function ( ’f ’ , [ states , controls ] , [ rhs ]) # nonlinear mapping
function
35
36 # Decision Variables ( controls )
37 U = ca . SX . sym ( ’U ’ , n_controls , N )
38
39 # Parameters including the initial and the reference state of the
robot
40 P = ca . SX . sym ( ’P ’ , n_states + n_states )
41
42 # State Matrix
43 X = ca . SX . sym ( ’X ’ , n_states , N +1)
44 X [: , 0] = P [:3] # initial state
45
46 # Compute solution symbolically
47 for k in range ( N ) :
48 st = X [: , k ] # states
49 con = U [: , k ] # control effort
50 f_value = f ( st , con ) # vx , vy , omega
51 st_next = st + T * f_value # time * velocity information
52 X [: , k +1] = st_next
53
54 obj = 0 # Objective function

27
55 g = [] # constraints vector
56
57 # Decide the cost of the control input and the states
58 Q = np . zeros ((3 , 3) )
59 Q [0 , 0] = Qx
60 Q [1 , 1] = Qy
61 Q [2 , 2] = Qtheta # Weighting matrices ( states )
62
63 R = np . zeros ((2 , 2) )
64 R [0 , 0] = Rlin
65 R [1 , 1] = Rang # weighting matrices ( controls )
66
67 Q_path = Qpath # Weight for distance to gui_path
68
69
70 # Compute objective
71 for k in range ( N ) :
72 st = X [: , k ] # states
73 con = U [: , k ] # control effort
74
75 # Calculate the distance to the gui_path
76 d_path = distance_to_path ( st [1] , st [2] , path [: , 0:2])
77
78 # Calculate the objective
79 obj += ( st - P [3:6]) . T @ Q @ ( st - P [3:6]) + con . T @ R @ con +
Q_path * d_path **2
80
81 # Compute constraints
82 for k in range ( N ) :
83 g . append ( X [0 , k ]) # state x
84 g . append ( X [1 , k ]) # state y
85
86 # Make the decision variables one column vector
87 OPT_variables = ca . reshape (U , 2* N , 1)
88 nlp_prob = { ’f ’: obj , ’x ’: OPT_variables , ’g ’: ca . vertcat (* g ) , ’p ’:
P}
89
90 # Iterative point method used for nonlinear programming
91 opts = { ’ ipopt . max_iter ’: 100 , ’ ipopt . print_level ’: 0 , ’ print_time ’:
0,
92 ’ ipopt . acceptable_tol ’: 1e -8 , ’ ipopt .
a c c e p t a b l e _ o b j _ c h a n g e _ t o l ’: 1e -6}
93
94 # Solver is created from the objective function and iterative point
method nonlinear programming
95 solver = ca . nlpsol ( ’ solver ’ , ’ ipopt ’ , nlp_prob , opts )
96
97 # Inequality constraints ( state constraints )
98 lbg = 0.3 # -15 # lower bound of the states X and Y
99 ubg = 1.45 #15 # upper bound of the states x and y
100
101 # Input constraints
102 # Define bounds
103 lbx = np . full ((2* N , 1) , v_min )
104 ubx = np . full ((2* N , 1) , v_max )
105
106 # Adjust bounds to enforce minimum speed
107 lbx1 = np . ones (( N ,1) ) * v_min
108 lbx2 = np . ones (( N ,1) ) * omega_min
109 lbx = np . concatenate (( lbx1 , lbx2 ) , axis =1)

28
110 lbx = lbx . reshape ( -1 ,1)
111
112 ubx1 = np . ones (( N ,1) ) * v_max
113 ubx2 = np . ones (( N ,1) ) * omega_max
114 ubx = np . concatenate (( ubx1 , ubx2 ) , axis =1)
115 ubx = ubx . reshape ( -1 ,1)
116
117
118 def __init__ ( self , path_array , init_conds ) :
119
120 # Initialise desired gui_path and initial conditions
121 self . x0 = init_conds [: ,0] # [ x ; y ; theta ]
122 self . path = path_array # [ xs1 , ys1 , theta1 ; xs2 , ys2 , theta2 ...]
123 self . path_step = 0
124 self . xs = np . transpose ( self . path [ self . path_step , :]) # [ xs1 ; ys1
; theta1 ]
125 self . xx = self . x0 # Current Position will be updated
126 self . u0 = np . zeros (( MPC_class .N , 2) ) # two control inputs
127 self . u_apply = self . u0 [0 ,:]
128
129
130 self . integral = 0
131 self . totalTime = 0
132 self . t0 = 0
133
134 self . iterations = 0
135
136 self . p = np . concatenate ([ self . x0 , self . xs ]) # set the values of
the parameters vector
137 self . x0_args = self . u0 . reshape ( -1 , 1) # initial value of the
optimisation variables
138
139 self . sol = MPC_class . solver ( x0 = self . x0_args , lbx = MPC_class . lbx ,
ubx = MPC_class . ubx , lbg = MPC_class . lbg , ubg = MPC_class . lbg , p =
self . p )
140
141 self . path_complete = False
142
143 def mpc_solve ( self , current_position ) :
144
145 # Update current position and check if reached gui_path end
146 self . x0 = current_position [: ,0]
147 if ( self . x0 [2] <0) :
148 print (" Recieved negative angle ")
149 # print ( self . x0 )
150
151 # Check for angle overflow
152 diff = self . xs [2] - self . x0 [2]
153 diff = ( diff + np . pi ) % (2* np . pi ) - np . pi
154 self . x0 [2] = self . xs [2] - diff
155
156 # check tolerances
157 error = self . x0 - self . xs
158 errorprint = error [2]*180/ np . pi
159
160 print ( f " error : { error [0]:.3 f } ,{ error [1]:.3 f } ,{ errorprint :.3 f } ,
iterations : { self . iterations }")
161
162 if ( self . path_step == len ( self . path [: ,1]) -1) :

29
163 tolerance = np . array ([ tolx_final , toly_final , toltheta_final
])
164
165 else :
166 tolerance = np . array ([ tolx , toly , toltheta ])
167
168
169 finished = ( abs ( error [0]) < tolerance [0]) & ( abs ( error [1]) <
tolerance [1]) & ( abs ( error [2]) < tolerance [2])
170 if ( finished | ( self . iterations > MPC_class . max_iterations ) ) :
171 self . iterations = 0
172 # Update gui_path and check if end
173 self . path_step += 1
174 print ( f "\ nreached point { self . path_step }\ n ")
175
176 if ( self . path_step == len ( self . path [: ,1]) ) :
177 # gui_path reached end set finish flag
178 self . path_complete = True
179 print (" MPC finished ")
180 else :
181 self . xs = np . transpose ( self . path [ self . path_step , :])
182
183 else :
184
185 # print ( f " self . xs : { self . xs }")
186 self . p = np . concatenate ([ self . x0 , self . xs ]) # set the
values of the parameters vector
187 self . x0_args = self . u0 . reshape ( -1 , 1) # update value of the
optimisation variables
188 # Solve for optimal control effort
189 self . sol = MPC_class . solver ( x0 = self . x0_args , lbx = MPC_class .
lbx , ubx = MPC_class . ubx , lbg = MPC_class . lbg , ubg = MPC_class .
ubg , p = self . p )
190 # print ( MPC_class . solver . stats () [" iter_count "])
191 # print ( MPC_class . solver . stats () [" return_status "])
192 solved = MPC_class . solver . stats () [" return_status "]
193 # Control effort using optimal values of the decision
variables
194 optimal_ctrl = ca . DM ( self . sol [ ’x ’ ]) . full ()
195 u = optimal_ctrl . reshape ( MPC_class .N ,2)
196
197 # Update previous control values u0 and iterations
198 self . u0 = np . vstack (( u [1: MPC_class .N , :] , u [ MPC_class .N -1 ,
:]) )
199 self . iterations += 1
200
201 # Set desired control signal
202 self . u_apply = u [0 ,:]
203
204 write_to_csv ( current_position [: ,0] , self . xs , self . u_apply ,
solved )
205
206 return [ self . path_complete , self . u_apply ]

30