Design and implementation of proximal planning and control of

an unmanned ground vehicle in the dynamic environment

This paper was downloaded from TechRxiv (https://www.techrxiv.org).

LICENSE

CC BY 4.0

SUBMISSION DATE / POSTED DATE

19-07-2022 / 22-07-2022

CITATION

Khan, Subhan; Guivant, Jose (2022): Design and implementation of proximal planning and control of an
unmanned ground vehicle in the dynamic environment. TechRxiv. Preprint.
https://doi.org/10.36227/techrxiv.20336373.v1

DOI

10.36227/techrxiv.20336373.v1
 Design and implementation of proximal planning
and control of an unmanned ground vehicle in the
dynamic environment
Subhan Khan, Jose Guivant, Member, IEEE,

Abstract—This paper presents a novel proximal planning and area. Therefore, motion planning and control are critical part 18

control (PPC) formulation for an unmanned ground vehicle of the ground vehicles. 19
(UGV) affected by skidding and slip disturbances. The control Proximal algorithms are advantageous for resolving com- 20
approach also considers the presence of moving and static
obstacles in the context of operation. The PPC technique is plex optimisation problems, particularly those with non- 21

divided into three parts; first, a nonlinear model predictive smooth or composite objective functions. A proximal algo- 22

control (NMPC) based path-planner is designed to periodically rithm is one whose fundamental iterations involve the proximal 23

generate an updated feasible trajectory to prevent the vehicle operator of some function and whose evaluation involves 24
from colliding with other objects from start to the goal pose. solving a specialised optimisation issue that is often simpler 25
In particular, a proximal averaged Newton-type method for
optimal control (PANOC) is used to design NMPC. Second, than the original problem [5]. In optimization theory, numer- 26

the velocity commands are produced via evolutionary program- ous techniques have been used with MPC, For instance, in 27

ming (EP) based kinematic control (KC). Third, a dynamic [6], a decentralised control strategy based on solving two 28

control mechanism with an extended state observer (ESO) is convex quadratic programming (QP) problems is described to 29
introduced to calculate the impacts of unidentified bounded determine the order in which agents traverse the intersection. 30
perturbations. Finally, simulations are performed in the presence
of linear and nonlinear trajectories of moving obstacles (MO) and In [7], an MPC-based approach addressed a high-level time 31

static obstacles (SO) to verify the performance of the proposed slot allocation optimum control problem (OCP), with agent 32

scheme. Additionally, we have investigated and confirmed that controls dictated by a layered low-level OCP. In essence, 33

the proposed PPC could run in real-time on a CPU with limited each agent optimises a quadratic programme (QP) and two 34
resources. linear programmes (LP) and sends the results to the central 35

Index Terms—Evolutionary programming, Extended state ob- coordinator, who solves the high-level nonlinear problem 36

server, Nonlinear model predictive control, Proximal planning (NLP). Rear-end collision prevention is added in [8], which 37
and control, unmanned ground vehicle improves the platform’s performance. A similar approach is 38

provided in [9], where agents are governed decentrally, all 39

I. I NTRODUCTION OCPs are convex QPs, and a central coordinating unit is in 40

charge of time slot allocation. Additionally, a decentralised 41

1

3
U NMANNED Ground Vehicles (UGVs) have gained pop-
ularity over the past few decades in civilian, commercial,
and defense applications. Driving a UGV on rough terrain is
consensus-based control system selects vehicle controls at
the lower level by solving a convex optimisation problem
42

43

after using a high-level consensus algorithm to decide the 44

4 not an easy task. In particular, dealing with uncertainties and
order of junction crossings. Collision avoidance is achieved 45
5 disturbances is challenging for the vehicles. These uncertain-
by restricting the distances between agents from below [10] 46
6 ties and disturbances could cause skid-slip. Consequently, to
instead of using time slots. Furthermore, in [11], a distributed 47
7 complete a task, a UGV should be capable of controlling and
MPC technique is implemented, and each agent solves a 48
8 navigating in an uncontrolled environment [1].
nonconvex quadratic programming problem (QCQP). Agents 49
9 It is essential to understand the proper operational proce-
must maintain a minimal distance between themselves as 50
10 dures and safety precautions before using an UGV. An operator
determined by their mutual collision point rather than relying 51
11 or automatic control of bulldozer blades is required to prevent
on time slots. Thus, it motivates us to use PANOC in [12–14]. 52
12 measurement errors [2, 3]. The safe and reliable operation
Additionally, a Casadi-based frame work ([15]) is employed 53
13 of bulldozers also requires a path-follower controller [4] to
to compare the proposed PPC with single shooting (SS) and 54
14 drive in following a specific path; for this purpose, proper
multiple-shooting (MS) numerical methods. 55
15 control actions are necessary for achieving that purpose while
16 still treating equipment adequately. Additionally, manual or A. Related Works 56
17 automatic bulldozers should avoid collisions in the operational
The motion control of UGVs has advanced significantly dur- 57

S. Khan (e-mail: Subhan.Khan@sydney.edu.au) and J. Guivant (e-mail: ing the last few decades. Problems with posture stabilisation 58

J.guivant@unsw.edu.au) are with the School of Mechanical and Manufac- and trajectory tracking were researched concerning vehicle 59
turing Engineering, University of New South Wales (UNSW) Sydney, NSW, control (such as [16–20]). The co-design problem of targeting 60
Australia. In addition, Subhan Khan is also with the school of Electrical and
Information Engineering at the University of Sydney. posture and tracking control of UGVs was also studied [21– 61

Corresponding Author: S. Khan 23]. MPC has also been used to handle physical constraints on 62
 2

63 state and input [24–26]. Some control techniques have consid- this purpose, a robust model predictive control is proposed 121

64 ered the control at a dynamic level while not needing UGVs to suppress the effects of uncertainties in [37], a disturbance 122

65 velocities [27, 28]. Most recently, some of the existing works observers (DO) are used to estimate the external disturbances 123

66 on autonomous bulldozers are based on high-level planning. and uncertainties [38, 39]. A robust tracking scheme DO that 124

67 A high-level planner is compared with manual operation in handles slip-skid is presented in [40]. The drawbacks of DO 125

68 [29], where the automatic operation can outperform the manual can be compensated using Nonlinear DO (NDO) [41]. ESO- 126

69 one. An efficient path-planner for autonomous bulldozers in based observations are used to estimate both internal and 127

70 [30, 31] proved to be essential for industries. external disturbances [42]. To estimate lumped disturbances, a 128

71 The above-mentioned existing techniques are considered generalized ESO (GESO)-based tracking at the dynamic level 129

72 without skid-slip. For low-and medium-level control problems, is presented in [40]. In [43], neural networks were used to 130

73 this pure rolling assumption may not hold for bulldozing ap- perform robust tracking under the presence of ski-slip in a 131

74 plications where the environment is uncontrolled. In addition, dynamic environment. Finally, the tracking control problem 132

75 safety hazards might occur in a real application. A solid is solved based on hybrid NDO and extended Kalman filter 133

76 investigation in controlling an UGV is required to mitigate any (EKF) presented in [44]. The proposed technique solves the 134

77 safety hazard. The ability of MPC to handle constraints and low-level control. However, it does not consider the skid- 135

78 predict the platform’s future response makes it an ideal choice slip presence model, which may still affect the closed-loop 136

79 in selecting it as a local planner. Some existing techniques performance. 137

80 have used MPC in the presence of wheel slip. For instance, In terms of motion planning of robotic systems, sampling 138

81 a min-max MPC for agriculture application [32], a traction based algorithms have significantly advanced the state of the 139

82 control problem on stabilizing wheeled mobile robot (WMR) art in robotic motion planning. For instance, When deter- 140

83 [33], a detailed analysis on controlling off-road vehicles [34], mining solution pathways, the planner takes uncertainty into 141

84 and a voltage control strategy based on MPC and nonlinear account, resulting in mobility that is safe in the presence of 142

85 disturbance observer (NDO) is implemented in [35]. For world model imperfections [45]. The planner directs sensing 143

86 industrial-grade applications, it is important to work without throughout the planning process in order to develop the world 144

87 colliding with a moving or static object. For this purpose, model. The experimental findings reveal that the suggested 145

88 bulldozing applications must be solved by considering the planning strategy enhances planning resilience in the presence 146

89 platform’s dynamics and kinematics, including skid-slip and of uncertainty greatly. In addition, in settings with dynamic 147

90 external disturbances. impediments, an integrated method combining the fast rapidly 148

91 A simulated technique using casadi-based ([15]) platform exploring random tree (RRT) algorithm and the configuration- 149

92 proposed in [20] deals with both skid-slip and obstacle avoid- time space may be employed to increase the quality of the 150

93 ance. They have also designed an NMPC-based planner, a planned trajectory and replanning [46]. An obstacle-cluttered 151

94 virtual kinematic controller, a saturated torque-based dynamic environment requires a search-based planning approach to 152

95 controller, and an extended state-observer. Although the results calculate dynamically possible routes for an autonomous robot. 153

96 are promising in this work, some issues require more investiga- This method explores the map using a collection of short- 154

97 tion. First, the prediction horizon of MPC used in [20] is large duration motion primitives to find smooth, minimum-time tra- 155

98 due to selecting the single-shooting method. In practice, the jectories [47].Furthermore, there are some numerical optimal 156

99 large prediction horizon could cause a slow processing time control based planners such as the multi-vehicle lane change 157

100 for the onboard processor in the industrial-grade application, motion planning job is a centralised optimum control issue, 158

101 which could be resolved with the multiple-shooting method. which is advantageous in that it is general and comprehensive 159

102 However, in real applications, it is important to investigate the [48], and a numerical technique for multiple-vehicle motion 160

103 processing time under different prediction horizons. Another planning that explicitly considers vehicle dynamics, temporal 161

104 solution to solve this problem is the proximal algorithms and geographical constraints, and energy needs [49]. 162

105 presented in [12, 36], which is a faster way to produce optimal

106 control problems if compared to quadratic programming (QP)
B. Contributions 163
107 or interior-point (IP) methods. In addition, the control actions
108 have aggressive in the presence of one moving obstacle. In This article focuses on the new PPC technique to reach a 164

109 addition, actual velocities applied to the platform are not destination position safely in the context of moving and static 165

110 presented or compared with the reference ones, which is obstacles, skid-slip, and external disturbances in an isolated 166

111 important to investigate to confirm that the predictive planner’s environment. The main contributions of this research work 167

112 constraints meet the actual ones. In [12, 36], obstacles and are: 168

113 physical limitations can be handled; the fast NMPC in the 1) Implementation of a proximal algorithm-based NMPC 169

114 presence of PANOC is good enough for solving the posture path-planner that incorporates environmental impedi- 170

115 control problem. However, they have not tested PANOC for ments and generates workable reference trajectories. 171

116 bulldozing applications by considering the dynamics of the 2) A dynamic controller connects to a kinematic controller 172

117 platform. Additionally, external disturbances and skid-slip are tuned by evolutionary programming to produce velocity 173

118 not considered in the previous work on PANOC. vector, which controls the platform using torque. 174

119 In tracking control of UGVs, the platforms are always 3) An observer from the ESO to calculate the lumped 175

120 subject to different internal and external disturbances. For disturbances (skid-slip and external torque disturbances). 176
 3

177 4) An investigation and comparison on the processing time

178 per iteration (PTI) of the proposed approach with [20].
179 In contrast [20], in this paper, we investigate predictive
180 planning and control of an UGV in the presence of skid-slip,
181 uncertainties, and disturbances in an uncontrolled environment
182 under the presence of moving and static objects. We use EP-
183 based gain tuning for KC to generate velocity commands,
184 a dynamic controller to get saturated torque, and ESO to
185 deal with the tracking problem and estimate the lumped
186 disturbance and compensated torque to converge the actual
187 velocity commands. An NMPC-based path planner is used
188 to construct a workable reference trajectory from start to
189 end. Additionally, to propose the best course of action for
190 performing map-based practical simulation, the pseudo priority
191 queues (PPQ) based Dijkstra algorithm is paired with NMPC, Fig. 1: Configuration of the UGV
192 which is also implemented in [50].

193 C. Outline where X consists the states of the platform, Ω is the velocity 204

vector, d is the distance between the centre of mass of the 205

194 This paper is organized as follows. In Section II, the prob-
UGV and wheel axis, J is the moment of inertia, radius 206
195 lem is formulated by discussing the kinematics and dynamics
of the wheel is r, η is the perturbed angular velocities due 207
196 of the UGV. The design PPC is discussed in Section-III,
to slipping of the two actuated wheels, η1 and η2 is the 208
197 which consists of NMPC-based path planning, EP-based KC,
longitudinal slip velocity and yaw rate perturbation due to 209
198 dynamic controller with ESO, and stability analysis of the
slippage of the wheels, respectively. Additionally, m is the 210
199 proposed scheme. The results of simulation experiments and
mass of the vehicle, M is the symmetric and positive-definite 211
200 investigation on the performance of the proposed PPC are
inertia matrix, Centripetal and coriolis matrices H1 (X, Ẋ), 212
201 given in Section IV. Finally, the conclusions and future works
H2 (X), and H3 (X). Furthermore, µ is the lateral skidding 213
202 are discussed in Section V.
velocity of the vehicle, τ is the applied torque, τd is the torque 214

disturbance vector, B is the input transformation matrix, and 215

203 II. P ROBLEM F ORMULATION ρ is the mismatched disturbance vector. 216

Figure 1 shows the configuration of UGV, which consists Assumption 1: [40, 53]: M , H1 (X, Ẋ), H2 (X), and H3 (X) 217

of positions (x, y) and heading (θ) as the states, while are bounded but unknown. In addition, the nominal value M0 218

[v, ω]T represents the longitudinal velocity and yaw rate of the is unknown. 219

platform. In this work, we have used the following kinematics Assumption 2: [43, 52, 54]: The skid-slip parameters and
and the dynamic models provided in [51, 52]: torque disturbances are unknown but bounded. Additionally
the platform is subject to the following constraints:
Ẋ = S(X) · (Ω − η) + ρ(X, µ) (1)
M · (Ω̇ − η̇) + H1 (X, Ẋ) · (Ω − η) + H2 (X) · ρ̇(X, µ) Ωrmin ≤ Ωr (k) ≤ Ωrmax ,
(2) (3)
+ H3 (X) · ρ(X, µ) = B · (Sat(τ ) − τd ) armin ≤ ar (k) ≤ armax
where Ωr is the reference velocity vector and ar is the
with,
reference acceleration vector of the platform. The UGV has to
X = [x, y, θ]T ∈ R3 , Ω = [v, ω]T , η = [η1 , η2 ]T ∈ R2 , avoid dynamic and static obstacles to reach the goal location
r for the posture stabilisation control. Therefore, the following
τd = [τd1 , τd2 ]T ∈ R2 , η1 = (ηr + ηl ),
2   obstacle avoidance constraints can be formulated as:
r 0 −mdθ̇ q
η2 = (ηr − ηl ), H1 (X, ẋ) = , Doh (X) = (x − xho )2 + (y − yoh )2 , (4)
2b mdθ̇ 0
grh (X) = Doh (X) − (r + roh ) ≥ 0 (5)
m + 2J 2J
 
H2 (X) = r 2 cos θ (m + r 2 ) sin θ 0
2 , where h = 1, 2, 3, . . . is the number of obstacles in the
−md sin θ md cos θ J + 2br2J environment, Radii of the obstacles is ro . The obstacles have
 
−2J θ̇ sin θ 2J θ̇ cos θ xand y coordinates as xo and yo . The motion of UGV is
r2 r2 −mdθ̇
H3 (X) = ,
0 0 0 restricted to work in a domain defined as
 T
 T −µ sin θ X∈Ψ (6)
cos θ sin θ 0
S(X) = , ρ(X, µ) =  µ cos θ  ,
0 0 1 Thus, we can summarize the objective of this work is as 220
0
  follows: Design a kinematic control law for Eq. (1) and 221
m 0 saturated torque control for Eq. (2) such that X can reach
M= , τ = [τ1 , τ2 ]T , ∈ R2 . 222
0 J − md2 the desired destination by respecting the constraints imposed 223
 4

224 on platform (saturated torque, longitudinal velocity, acceler- Now, we are in a position to define the cost function of our
225 ation and yaw rate limitations given in Eq.(3) and collision point stabilisation problem as follows:
226 avoidance mentioned in Eq. (4) and Eq. (5).
arg min J(ar ) =
ar
227 III. PPC METHOD FOR UGV N
X −1
LN (XN ) + Lk (Xr (k + i), Ωr (k + i), ar (k + i))
228 In this section, first, we will discuss the design NMPC- i=0
229 based path-planner, which uses PANOC. Second, velocity- (9)
230 based control commands are generated by EP-based KC.
231 Finally, the dynamic controller, ESO, and stability analysis subject to following constraints
232 of the proposed scheme are dicussed.
Xr (k + i) = f1 (Xr (k + i − 1), Ωr (k + i − 1)), (10a)
Ωr (k + i) = f2 (Ωr (k + i − 1), ar (k + i − 1)), (10b)
233 A. NMPC-based path-planner
ak (k + i − 1) ∈ Uk , i ∈ N[0, N −1] , (10c)
Consider the platform operating in a context, with the
presence of dynamic and static objects. The UGV’s initial pose grh (Xk ) ≤ 0, i ∈ N[0, N ] (10d)
is X(0) and goal pose is Xg under physical limitations Eq. (4)
and Eq. (5). The reference posture and velocity can be denoted The stage cost: Lk : Rnp × Rnu → R could be taken as a 237

as Xr = [xr , yr , θr ]T and ωr = [vr , ωr ]T , respectively. We quadratic function in the form of L(Xr (k), Ωr (k), ar (k)) = 238

are considering the reference robot is pure rolling (excluding (Xr − Xg )T · Qk · (Xr − Xg ) + ΩTr · Q̄k · Ωr + aTr · R · ar . In 239

disturbances). So, our kinematics and dynamic models for the addition, the terminal cost LN : Rnx → R can be defined as 240

reference robot described in [53] LN (XN , ΩN ) = (XN −Xg )T ·QN ·(XN −Xg )+ΩTN Q̄N ΩN . 241

The parameter N is the prediction horizon, Xg is the initial 242

Ẋr = S(Xr ) · Ωr , states of the platform, Q and Q̄Semi-positive state-penalty gain 243
(7)
Ω̇r = ar matrix, and R is the diagonal positive definite input-penalty 244

gain matrix. An adjoint method can be used to obtain the 245

where ar = [v̇ , ω̇]T . Up until this point, we have assumed that gradient of the function L in Eq. 9 as shown in the Algorithm 246

the UGV operates in continuous time. However, a computer 1 obtained from [55], it allows the projected gradient iteration. 247

must produce discrete signals in order to function. Therefore, Now, we can use the adjoint method and convert it to
we may approximate our nonlinear continuous-time system produce forward-backward splitting (FBS) scheme:
into a discrete-time system as follows by applying Euler’s
approximation with a sampling time Ts : aν+1 = Tγ (aνr ) := proxγ (aνr − γ∇J(aνr )), (11)
r
1
Xr (k + 1) = Xr (k) + Ts · Ẋr (k) proxγ (ar ) = arg min kar − ξk2 . (12)
(8) ξ 2γ
Ωr (k + 1) = Ωr (k) + Ts · ar (k)
where aν+1
r consists of all accumulation points, γ-critical 248
234 where Xr (0) = X(0) and Ωr = Ωr (0) are taken as the
points always satisfy γ > 0, and ξ = γ∇J(aνr ). The 249
235 beginning conditions. The discrete indexing k ∈ Λ is a
fixed points Tγ could have a relationship with the following 250
collection of positive natural integers.
accumulation points as a∗r = Tγ (a∗r ). 251

Algorithm 1 Adjoint method for J(ar ) in the Eq. (9)

1: Input: (Xo , Ωo ) ∈ R5 , ar ∈ R2 B. NMPC Proximal reformulation 252
2: Output: J(ar ), ∇J(ar )
3: J(ar ) ← 0 A fixed-point residual operator can reduce the fixed point
4: while k < N do problem Tγ to the following one, which gives an intuition of
5: (Xr (k + 1), Ωr (k + 1)) = fk (Xr (k), Ωr (k), ar (k)) using Newton-type iterative scheme:
6: L(ar ) ← L(ar ) + L̄k (Xr (k), Ωr (k), ar (k))
7: L(ar ) ← L(ar ) + LN (Xr (N ), Ωr (N )) 1
Rγ = (ar − Tγ (ar )) (13)
8: Xk ← ∇LN (Xr (N ), Ωr (N )) γ
9: X̃k = ∇(Xr (k), Ωr (k)) · fk (Xr (k), Ωr (k)) · X̃k+1 + aν+1
r = aνr − Hν Rγ (aνr ) (14)
∇(Xr (k), Ωr (k)) · L̄k (Xg (k), Ωr (k), ar (k))
10: ∇ar ← ∇J(ar )fk (Xr , Ωr (k), ar (k)) + to encode first-order fixed-point residual Rγ , an invertible
∇ar (k)L(Xr (k), Ωr (k), ar (k)) linear operator Hν is appropriately chosen by applying in-
11: k ←k+1 verse secant condition obtained by quasi-Newtonian methods
12: end while provided in [56].In addition, only guarantee of neighboring γ
236 critical points of a∗r , we have used forward backward envelope
 5

Fig. 2: Block representation proposed PPC method

(FBE) which is described in [13, 57] for Eq. 9 which can be Only the fitness function of a single population is necessary
defined by quadratic upper bound as follows: to understand evolutionary programming. Suppose P be the
population of all the parameters in Eq. (1), compensated state
Qγ J (ξ; ar ) = proxγ (ar ) + J(ar ) + ∇J(ar )T (ar − ξ) (15) gains are referred as k ∗ = [kˆx kˆy kˆθ ] ∈ R3 . The main goal of
ϕγ (ar ) = inf Qγ J (ξ, ar ), this exercise is to identify scaling factors kˆx , kˆy and kˆθ that
ξ∈U
γ are not ideal.
ϕγ (ar ) = L(ar ) − k∇J(ar )k2 (16)
vr cos(e3 ) + kˆx e1
   
2 v̂
ûc = c = (21)
+ dist2U (ar − γ∇J(ar )) ω̂c ωr + kˆy vr e2 + kˆθ vr sin(e3 )
253 The above expressions show the similar complexity with FBE The following formula can also be used to calculate the error
254 and forward-backward step. To solve the problem of Eq. (9), function for velocities ue = [ve , ωe ]T ∈ R2 :
255 we have employed PANOC, a fast nonlinear MPC introduced ve = −xe sin θ + ωye cos θ + v − vr cos θe (22)
256 in [13]. The advantage of PANOC over interior point method
257 or quadratic programming, is in terms of the processing time ωe = ω − ωr (23)
258 required at each iteration of the MPC process. Finding sub-optimal scaling factors for kˆx , kˆy and kˆθ is

Algorithm 2 Evolutionary control algorithm

259 C. EPKC design
1: procedure EPKC(P0 , Copt , N )
Through the use of evolutionary approaches, the degree of
2: t̄ ← 0
freedom in the instantiation of meta-heuristic algorithms is
3: ī ← 0
decreased. The important idea is to take into consideration
4: repeat
any variations between the reference and actual positions and
5: while ī ≤ PN do
orientations. N
    6: nr ← ī=1 (W1 e1ī )2 + (W2 e2ī )2 + (W3 e3ī )2
xe xr − x 7: G ← variation(n1 , . . . , nk )
X̃e = Xr − X =  ye  =  yr − y  (17) 8: Et ← Et ∪ n
θe θr − θ 9: end while
where Xe is the error function between reference and actual 10: P t̄+1 = selection of best Cl individuals from E t̄ ∪
t̄
states of the platform. We now take the actual UGV’s posture (n ∈ P )
error from Eq. (17 by using the global transformation indicated 11: t̄ ← t̄ + 1
in [58]. 12: until stopping criterion fulfilled
      13: end procedure
e1 cos(θ) sin(θ) 0 xe
260
Xe = e2  = −sin(θ) cos(θ) 0 ·  ye  (18) the major goal here. The EP searching algorithm is shown in 261
e3 0 0 1 θe Algorithm 2, which consists of weighting values W1 , W2 , W3 262

The time derivative of tracking error of Eq. (18) is as follows of platform’s states and performance index of integral square 263

        error (ISE) in the populations n1 , . . . nk , G is the variation of 264

e˙1 −1 e2 vr cos(e3 ) ISE, and E is the shared policy for each process state, can be 265
e˙2  = v  0  + ω −e1  +  vr sin(e3 )  (19) used to find these sub-optimal scaling factors. 266

e˙3 0 −1 ωr
Therefore, a back-stepping technique is employed to obtain D. Design of a dynamic controller and an ESO 267

control signals that do not include skid-slip [? ]. The following equation can be constructed from the dy-
    namic equation in Eq. (2):
vc vr · cos(e3 ) + kx · e1
uc = = (20)
ωc ωr + ky · vr · e2 + kθ · vr sin(e3 ) M̄ · û˙ c + F = B̄ · τ (24)
 6

with t ∈ [0, ∞) such that the first derivative holds the condition 285

x sin θ − ye cos θ 1
 limt→∞ α˙1 (t) = 0. 286
M̄ = T T · M · T, B̄ = T T B, T = e , Theorem 1: Consider the model of UGV Eq. (1) and Eq.
1 0
(2), the posture control error Eq. (18) and Eq. (20), the ESO
F = T T [M · Ṫ (ue + Π) + M · T Π̇ − M · η̇ + H1 (v − η)] Eq. (28)- . (33), and controller Eq. (33). Then a compensated
+T T [H2 · ρ̇ + H3 · ρ] + B̄ · τd , error-inequality as follows
Π = [vr · cos θe + ωr · (xe · sin θ − ye · cos θ), ωr ]T Cmax
kec 2 k ≥ (Kcmin > kH¯m k) (34)
268 As per Assumption 2 we consider that the parameter M is Kcmin − kH¯m k
269 unknown, while nominal M0 is known, and error occurred on
the above inequality holds by adjusting the parameter, which 287
270 velocities could be defined as δ = u − ûc , we now have the
can get uniform aysmptotic stability of both e and ec 288
271 following expression
while,
−1
δ̇ = M̄0 B̄ · sat(τ ) + C · B̄ · sat(τ ) − M̄ −1 · F − û˙ c (25)
H̄m = H1 (X, Ẋ) · (Ω − η) + H2 (X) · ρ̇(X, µ)+
−1 −1 (35)
where C = M̄ − M̄0 . To address the saturation limi- H3 (X) · ρ(X, µ)
tations, we may also use ∆τ = sat(τ ) − τ . Additionally, it
will suggest that û˙ c is connected to an unidentified amount of Proof 1: . Consider the following Lyapunov function
skid-slip. Consequently, the above phrase takes the following V1 = k1 (ex + l(1 − cos(eθ ))2 + k1 (ey − l sin(eθ ))2
form:
+2k3 vr (1 − cos(eθ )) + V2
−1
δ̇ = M̄0 B̄ · τ + ∆ · τ + C · B̄ · sat(τ ) − M̄ −1 · F (26) (36)
−1
− û˙ c − (M̄0 B̄ − I)∆τ eT M̄ e
where V2 = c 2 c . Additionally, only when e = 0 and ec = 0 289

To accomplish the ESO, we first need to lumped the distur- is it known that the Lyapunov function V1 ≥ 0 and V1 = 0. 290

bances as follows Taking the subsequent derivative now 291

−1
zD1 = M̄ −1 · F − ˆ˙uc + (M̄0 B̄ − I) · ∆τ, (27)
V˙1 = 2k1 (ex + l(1 − cos(eθ ))(ėx + l sin(eθ )ėθ )
zD2 = C · B̄ · sat(τ ), (28)
+ 2k1 (ey − l sin(eθ )(ėθ − l cos(eθ )+ (37)
zD = zD1 + zD2 . (29)
2ke vr sin(eθ )ėθ + V̇2
272 Assumption 3: [59]: The first-order derivative of zD exists
273 and it is bounded. Let the function of Eq. (26) has an error eF = F − F̂ , and
The expression in Eq. (26) can now be rewritten as controller of Eq. (33), we can differentiate V2 and get
−1
δ̇ = M̄0 B̄ · τ + ∆τ + zD (30) 1
V˙2 = eTc (M̄˙ − 2H¯m ) · ec + eTc · (−Kc · ec + eF ) (38)
2
For the above system, the ESO design is as follows
It is crucial to note that the first term is zero due to the skew
ˆ −1
symmetry characteristic in this case, allowing us to recast the
δ̇ = M̄0 · B · τ + ∆τ + ẑD + LA · (δ − δ̂) (31)
V˙2 as
żˆD = LB · (δ − δ̂) (32)
V˙2 = eTc (−Kc ec + eF ) ≤ (−Kcmin kec 2 k
where δ̂ and ẑD are the estimations of δ and zD , respectively. (39)
LA and LB are observer gains. Thus, the dynamic control + kHm kkec 2 k + kec kCmax )
input τ can be defined as follows where Kcmin is the minimum value of Kc and Cmax is the
−1 β δ 2 maximum ESO estimate error. Now that the inequality and
τ = M̄0 · B̄ · (−α · δ + β · Kc −− ẑD ) (33) derivatives of the velocity error have been replaced in Eq.
2
(22) and Eq. (23),, we obtain
274 where Kc is the input saturation’s compensation gain, β is the
275 controller gain, and α is a diagonal positive definite matrix. V˙1 ≤ 2k1 (ex + l(1 − cos(eθ ))(vr cos(eθ ) − vc )+
276 The observer’s stability analysis is carried out in accordance (2k1 vr (ey − l sin(eθ )) + 2k3 vr · (ωr − ωc ) sin(e3 )−
277 with [20]. As a result, the following subsection will cover the
278 closed loop stability analysis of the controller design. Kc kec k2 + kH¯m kkec k2 + kec Cmax k
min
(40)

279 E. Stability analysis Now, we will introduce EPKC from Eq. (22), defining kˆ2 =
k1
280 We shall specify a key assumption before providing the kˆ v
and substitute in Eq. (38).
3 r
281 stability analysis of the closed-loop system in Figure (2):
282 Assumption 4: [60]: Consider a first order continuous dif- V˙1 ≤ −2k̂12 (ex + l(1 − cos(eθ ))2 − 2k̂32 vr2 sin(eθ )2 + kec k2
283 ferential variable with a limit of α1 (t) ∈ [0, ∞). The second (−Kcmin + kHm k) + kec Cmax k
284 derivative of α(t) is therefore present and constrained for every (41)
 7

It demonstrates that the first two components of the equation A. Dynamic collision avoidance 314

mentioned earlier are negative, guaranteeing that V˙1 is negative The intuition behind this simulation is to reach a goal 315
as long as the conditions are met. location while avoiding multiple moving obstacles having a 316

linear trajectories in the environment. The initial location 317

Cmax
kec k ≥ (42) is considered as Xinit = [0, 0, 0] and the goal location 318
Kcmin − kHm k
Xgoal = [3m, 4m , 0deg]. We have considered four obstacles 319

Consequently, we can describe the difference in velocity with the information of their location, speed, and radius. In 320

between EPKC and the estimated velocity as this simulation, the velocity and acceleration constraints are 321

as follows: 322
   
v̄ (vr cos(eθ ) + k1 (ex + l(1 − cos(eθ ))) − v̂ −0.45m/s ≤ v ≤ 0.45m/s, −45deg/s ≤ ω ≤ 45deg/s,
ec = e =
ω̄e (ωr + k2 vr (ey − l sin(eθ ) + k3 )vr sin(eθ ) − ω̂ −3m/s2 ≤ a ≤ 3m/s2 , −45deg/s2 ≤ ar2 ≤ 45deg/s2
r1
(43)
1) Obstacles with linear trajectories: Figure 3a shows the 323

Bothkek and kėk are bounded since the error function es = collision avoidance of the UGV with the moving obstacle 324

[e ec ]T is bounded. Additionally, the Assumption 4 leads to having radius 0.2 at a starting position of (x, y) = (3.5, 1.5), 325
0
limt→∞ V˙˙1 = 0. The posture and velocity errors of the UGV driving at a speed of 0.40m/s with the heading of θ = 180 . 326

are therefore said to be asymptotically stable. Since we are using an NMPC-based planner that can predict 327

the obstacle’s future location and the vehicle. Therefore, when 328

lim ea = 0, lim eb = 0 (44) it predicts the incoming of the obstacle, the UGV turns left 329
t→∞ t→∞ to avoid the collision. The vehicle slows down to avoid the 330

Now, substituting equality Eq. (43) into equality Eq. (42) and incoming obstacle having a speed of 0.35m/s0 approaching 331

inequality Eq. (40), we get the following from (x, y) = (4.0, 0) with a heading θ = 135 as shown in 332

Figure 3b. 333

k1 (ex + l(1 − cos(eθ )) + k3 vr sin(eθ ) = 0 (45) The reverse action of UGV is of importance as in the actual 334

case; a driver can stop the vehicle and give way to the obstacle. 335

which follows the It can be improved by increasing the prediction horizon length 336

or refining the cost function with suitable penalization of 337

lim ex = 0, lim eθ = 0 (46) longitudinal velocity and yaw rate. Similarly, the UGV has 338
t→∞ t→∞
avoided the collision with another obstacle coming with a 339

Similarly, by the above expressions, there exists speed of 0.1m/s, as shown in Figure 3c, the vehicle’s turning 340

represents a situation when we have to take a safe turn. Finally, 341

lim ey = 0 (47) Figure 3d illustrates, the UGV has safely reached the goal 342
t→∞ location and stopped (parked). Figure 4a shows that the 343

292 Thus, the position and velocity error of the vehicle are saturated input torque has reasonably respected the saturated 344

293 uniformly asymptotically stable. constraints to comment on the proposed control structure’s 345

robustness. The slight chattering in the input torque is due 346

to a change in the heading to avoid the collision. Once the 347

294 IV. R ESULTS AND DISCUSSIONS UGV has avoided the collision, the input torque converges to 348

zero. It is crucial to investigate the reference velocity profile 349

295 The simulation tests to verify the resilience of the suggested produced by the NMPC and the velocity applied by the EPKC. 350

296 strategy are covered in this part. The following physical For this purpose, Figure 4b illustrates, the EPKC has followed 351

297 parameters are established: b = 0.76 m, r = 0.29 m, the proposed velocities with little chattering. The reason for 352

298 d = 0.59 m, m = 49 kg, mw = 1.9kg, Jc = 14 kgm2 , linear velocities to suddenly slow down 7.5s is due to obstacle. 353

299 J = 0.008 kgm2 , Jm = 0.0034 kgm2 , .The skid-slip are set as The proposed accelerations generated by the NMPC are shown 354

300 [η1 , η2 , µ] = 0.03 · [sin(0.2t), 1, 1] and external disturbance in Figure 4c, which shows the smoothness in the control action 355

301 td = [0.02 · cos(0.4t)0.02 · sin(0.4t)]T . For the dynamic in the linear acceleration. The angular acceleration has some 356

302 collision avoidance, the skid-slip parameters are employed as a fluctuations till 15s due to a change in the heading at specific 357

303 low-frequency piece-wise linear (PWL) function. The side slip points in the path. 358

304 angle β of the front wheel is calculated as β = arc tan( ẋẏ ). In Figure 4d shows the tracking errors of the UGV from the 359

305 terms of controller gain tuning, we have selected the weighting proposed NMPC-based trajectory and the actual one. The 360

306 matrices Q = diag[6, 11, 0.02], Q̄ = diag[0.02, 0.02], and Median of states are close to 0 while the standard deviation µ 361

307 R = diag[0.04, 0.04]. The prediction horizon was set to for x, y, and θ are 1cm, 1.3cm, and 0.009rad/s, respectively. 362

308 N = 14 for the experiment related to static obstacles, and a Additionally, certain outliers are also highlighted, which are 363

309 larger one of N = 19, for the context which include multiple respectable for the proposed simulation. It is important to 364

310 dynamic objects. Additionally, Ts = 0.01 s is chosen for the mention here that most tracking errors are within (−2 − 0)cm 365

311 sample time. The input torque is considered to be saturated and for the position and (−0.06 − 0)rad/s for the heading. 366

312 to have values of τmax = 10 Nm and τmin = −10 Nm. The 2) Obstacles with nonlinear trajectories: In a real envi- 367

313 subsections that follow address the input and state limitations. ronment, it is not necessary for an obstacle to have a linear 368
 8

5 5

UGV
4 Prediction 4
Trajectory
Goal
3 1.8 MO 3
TMO
1.6

y (m)

y (m)
2 2
1.4

1.2
1 1 UGV
Prediction
1
Trajectory
1.6 1.8
0 0 Goal
MO
TMO
-1 -1
0 0.5 1 1.5 2 2.5 3 3.5 4 0 0.5 1 1.5 2 2.5 3 3.5 4
x (m) x (m)

(a) (b)
5 5

4 4

3 3

y (m)
y (m)

2 2

UGV
1 UGV 1 Prediction
Prediction Trajectory
Trajectory Goal
0 Goal 0 MO
MO TMO
TMO
-1 -1
0 0.5 1 1.5 2 2.5 3 3.5 4 0 0.5 1 1.5 2 2.5 3 3.5 4
x (m) x (m)

(c) (d)
Fig. 3: (a) UGV slowing down to avoid collision from the dynamic obstacle approaching from (x = 3.5 , y = 1.5) with a heading of 180o .
(b) The vehicle is reversing and moving forward to prevent collision from an obstacle approaching from (x = 4.0 , y = 0) with a heading
of 135o . (c) UGV changing direction to avoid collision from the obstacle approaching from (x = 3.0, y = 4.0) with a heading of 255o . (d)
UGV stabilizes at the goal after avoiding dynamic obstacles.

15 0.5
(m/s)

10
0
1
Acutal longitudinal velocity
5 2
Reference longitudinal velocity
-0.5
10 Time(s) 15
(Nm)

0 5 20 25
0
50
Acutal angular velocity
-5
Reference angular velocity
(deg/s)

0
-10

-15 -50
0 2 4 6 8 10 12 14 0 5 10 15 20 25
Time (seconds) Time(s)

(a) (b)
1 0.08
Median
0.5
a r1 (m/s2 )

0.06
25%-75%
0 9%-91%
State Tracking errors

0.04 outliers
-0.5

-1 0.02
0 5 10 15 20 25 30
Time (s) 0
50
-0.02
a r2 (rad/s 2 )

-0.04
0

-0.06

-50
0 5 10 15 20 25 30 -0.08
Time (s) xe(m) ye(m) (rad)
e

(c) (d)
Fig. 4: (a) Saturated torque control applied to the platform in the presence of dynamic obstacles. (b)Actual and reference velocity commends
applied to the platform in the presence of dynamic obstacles. (c) Longitudinal and angular acceleration applied to the platform in the presence
of dynamic obstacles. (d) Platform’s state tracking errors, which shows the accuracy of the proposed scheme in the presence of dynamic
obstacles
 9

2 0.4

0.2
1.5

(m/s)
0

-0.2
1
-0.4
0 1 2 3 4 5 6 7 8 9

y (m)
0.5 Time(seconds)
Reverse
40
0 UGV
Prediction 20

(deg/s)
Trajectory
-0.5 Goal 0
MO
TMO -20
-1 0 1 2 3 4 5 6 7 8 9
0 0.5 1 1.5 2 2.5 3 3.5
Time(seconds)
x (m)

(a) (b)
Fig. 5: (a) The UGV avoiding an incoming nonlinear trajectory of MO (TMO). (b) The UGV approaching to goal location after avoiding
the collision with MOs. (c) Longitudinal velocity and yaw rate applied to the platform under the presence of nonlinear TMO.

300

100 250
198.5

200 199
200

y[m]
300
y[m]

199.5
150

400 200
Goal 100
Start
500 Trajectory
MO 200.5
50
UGV
600 SO
Prediction 201
0
200 400 600 800 1000 1200 739 740 741 742 743 744 745 746
x[m] x[m]
Longitudinal velocity[m/s]

1.5 0.03
Median
1
0.02
0.5
0.01
0

-0.5 0
0 100 200 300 400 500 600
Time[s] -0.01

40
Yaw Rate [deg/s]

-0.02
20
-0.03
0
-20 -0.04
-40
0 100 200 300 400 500 600 -0.05
xe ye
Time[s] e

Fig. 6: (a) Bird’s eye view of path-tracking of a large map (b) MO avoidance in the large map (c) Applied longitudinal velocity and yaw
rate to the platform (d) Box plot of path-tracking errors of individual states of the platform

369 trajectory. Consider a nonlinear trajectory case of the obstacles nature of trajectory of MO can suddenly approach the vehicle. 386

370 as shown in Figure 5a and 5b. The UGV has first completely Thus, it is crucial for the UGV to instantly make a decision 387

371 to stop to prevent a collision from both of the vehicles (also to prevent the collision. 388

372 took a reverse action due to the sudden occurance of nonlinear

373 trajectory of MO). The moment they pass, they can then
B. Tracking in a large map using PPQ-Dijkstra 389
374 proceed towards the goal location. In such cases, the prediction
375 horizon must have a more significant prediction to compute. A PPQ is a priority queue composed of unordered subsets of 390

376 Additionally, two situations could occur: 1) Either prediction entries. Each subset is made of objects whose current costs fall 391

377 horizon is large enough to allow the UGV to break and stop. 2) within a specified cost interval. The queue simply preserves the 392

378 The cost function must be modified as per the given situation, order of its items. In this tracking experiment, the University 393

379 i.e. the penalization must be refined on the internal input states. of New South Wales Wale’s evacuation map is considered, 394

380 In contrast, if these things are neglected, the vehicle might take and the tracking method is combined with a 2 DoF optimum 395

381 a reverse and then goes forward, which could cause a collision planner (PPQ-Dijkstra). The campus’s buildings are shown on 396

382 due to the nonlinear nature of the obstacles.The longitudinal the 1350 x 600 m map. The experiment’s goal is to steer clear 397

383 velocity and yaw rate applied to the UGV can be seen in of MOs and SOs on a big map. As shown in Figure 8a, three 398

384 the Figure 5c, which clearly indicates the reverse action taken destinations are designated for this purpose, with the UGV 399

385 by the UGV from (1-2)seconds. This is because the nonlinear able to select the quickest route from the PPQ-Dijkstra (which 400

can deal with multiple destinations). Figure 6a’s coloured lines 401
 10

Static obstacle avoidance Static obstacle avoidance

Prediction Horizon

Tracking error (cm)

2
150 SS [20]
1.5 MS [20]
100 PPC
1
50 SS[20]
MS [20] 0.5
15 PPC
0
0 20 40 60 80 100 N = 15 N = 50 N = 100 N = 150
Average NMPC processing time (ms) Prediction Horizon
Dynamic obstacle avoidance Dynamic obstacle avoidance

Prediction Horizon

Tracking error (cm)

6
150 SS [20]
MS [20]
4 PPC
100

50 SS[20] 2
MS [20]
15 PPC
0
0 20 40 60 80 100 120 N = 15 N = 50 N = 100 N = 150
Average NMPC processing time (ms) Prediction Horizon

(a) (b)
9
Multi-Core CPU
8 Single Core CPU

7
Average PTI (ms)
6

0
N = 15 N = 50 N = 100 N = 150
Prediction Horizon

(c)
Fig. 7: a) Comparing average PTI of the proposed PPC with single and multiple shootings method of IP at different prediction horizons. b)
Comparing PPC-based Euclidean distance tracking errors with single and multiple shootings method of IP at different prediction horizons.
c) PTI comparison of PPC, while running on a single-core CPU and multi-core CPU at different prediction horizons

402 depict the nominal cost of getting there (which implicitly prediction horizon, the error would be lower in PPC. Addition- 430

403 means approaching one of the valid destinations). For instance, ally, a detailed comparison of the optimization techniques is 431

404 red has a maximum value of 300, indicating a higher cost, provided in Figure 7b. The benefit of using prediction horizons 432

405 thus in order to follow the lowest cost, it is necessary to N > 100 is marginal in term of accuracy, as it can be 433

406 follow the automobile to blue (value 0). The vehicle’s non- appreciated in the table showing the errors. For computational 434

407 holonomic restrictions and collision avoidance are under the purposes, we have provided this comparison to save the PTI 435

408 control of the NMPC. Figure 6b shows the UGV avoiding of the onboard processor. 436

409 the MO as it approaches while also foreseeing the vehicle’s 2) Processing in single and multi-core CPUs: In terms 437

410 state. The velocity applied to the actual platform in terms of running the PPC in the single and multi-core CPUs, 438

411 of the effectiveness of the recommended system is shown in we have executed the PPC method with different prediction 439

412 Figure 6c. Chattering in the yaw rate and longitudinal velocity horizons. An increase in the prediction horizon could increase 440

413 greatly demonstrates environment collision avoidance. Figure the PTI due to an increase in the number of mathematical 441

414 6d shows that the actual states of the UGV are correct in terms computations. For instance, The Figure 7c shows that the PPC 442

415 of tracking errors. has a slight change in the PTI in both single-core CPU and 443

a multi-core one. This validates the proposed method to run 444

416 C. Performance analysis of the proposed scheme on a CPU with limited resources. Furthermore, it is evident 445

from Table 1, which specifies that PPC outperforms other 446

417 In this subsection, the performance of the path-planner and
numerical methods. Particularly, the tracking accuracy using 447
418 controller is evaluated based on the processing time of NMPC
PPC is better compared to the existing method highlighted in 448
419 and by analyzing the skid-slip.
420 1) PTI of the proposed PPC method: In terms of the [20]. 449

421 processing effort of the proposed scheme, Figure 7a shows

422 a comparison of PPC with the IP-based single and multiple V. C ONCLUSIONS 450

423 shooting method. In both the dynamic and static obstacles, the In this paper, a comprehensive novel PPC approach is 451

424 larger prediction horizon will have a more extensive processing proposed for designing the proximal formulation of an UGV. 452

425 cost, which is valid for practical cases. It is essential to The PPC consists of an NMPC-based path-planner, EP-based 453

426 mention that a prediction horizon of N = 100 has an almost KC, dynamic control law, and an ESO to handle the lumped 454

427 identical Euclidean distance error in PPC and IP methods disturbances. The proposed PPC was implemented on single 455

428 (SS and MS), with the former having less processing cost. and multi-core CPUs, which verifies that it has the ability to 456

429 Therefore, in regards to [20], using the same parameters and run on a CPU with limited resources despite an increase in 457
 11

TABLE I: Average P T /I for PPC running with PPQ-Dijkstra using single, multi-core CPUs, and a comparison of Euclidean distance
accuracy
N Single Core this paper Single Core [20] Multi-Core this paper Multi-Core [20] This paper Accuracy Accuracy [20]
15 1.2 ms 10.3 ms 1.1 ms 10 ms -2.0 cm–2.0 cm -10.0 cm–10.0 cm
50 6.12 ms 21.47 ms 5.35 ms 21 ms -0.45 cm–0.45 cm -2.5 cm–2.5 cm
100 7.80 ms 38.61 ms 7.17 ms 38 ms -0.04 cm–0.04 cm -0.25 cm–0.25 cm
150 8.22 ms 51.82 ms 8.79 ms 51 ms -0.01 cm–0.01 cm -0.01 cm–0.01 cm

458 the prediction horizon. The velocity commands and dynamic American Control Conference (ACC). IEEE, 2018, pp. 5994– 512

459 control signals have shown less chattering, which guarantees 6001. 513

460 the small Euclidean distance error. Simulation experiments [11] A. Katriniok, P. Kleibaum, and M. Joševski, “Distributed model 514

predictive control for intersection automation using a paral- 515

461 were performed by considering MOs and SOs. In both cases, lelized optimization approach,” IFAC-PapersOnLine, vol. 50, 516
462 the performance of the proposed scheme is robust and efficient no. 1, pp. 5940–5946, 2017. 517

463 in lowering the state-tracking errors in the presence of skid- [12] A. Sathya, P. Sopasakis, R. Van Parys, A. Themelis, 518

464 slip. G. Pipeleers, and P. Patrinos, “Embedded nonlinear model 519

predictive control for obstacle avoidance using panoc,” in 2018 520

465 Additionally, we have investigated the MOs with linear
European control conference (ECC). IEEE, 2018, pp. 1523– 521
466 and nonlinear trajectories, which identifies that for this sort 1528. 522
467 of situation, the cost function must be refined enough with [13] L. Stella, A. Themelis, P. Sopasakis, and P. Patrinos, “A simple 523

468 proper penalization on the internal inputs, and the prediction and efficient algorithm for nonlinear model predictive control,” 524

469 horizon’s length should be increased. The processing time of in 2017 IEEE 56th Annual Conference on Decision and Control 525

(CDC). IEEE, 2017, pp. 1939–1944. 526

470 our proposed scheme shows that the prediction horizon in PPC
[14] P. Sopasakis, E. Fresk, and P. Patrinos, “Open: Code gener- 527
471 is slightly better than SS and MS numerical methods, which ation for embedded nonconvex optimization,” arXiv preprint 528
472 also validates the convergence of proximal algorithms. For arXiv:2003.00292, 2020. 529

473 future work, we expect to validate our proposed scheme by [15] J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and 530

474 implementing it on an actual robot. M. Diehl, “CasADi – A software framework for nonlinear 531

optimization and optimal control,” Mathematical Programming 532

Computation, vol. 11, no. 1, pp. 1–36, 2019. 533

[16] C. Samson, “Control of chained systems application to path fol- 534

475 R EFERENCES
lowing and time-varying point-stabilization of mobile robots,” 535

476 [1] C. Samson, P. Morin, and R. Lenain, “Modeling and control IEEE transactions on Automatic Control, vol. 40, no. 1, pp. 536

477 of wheeled mobile robots,” in Springer handbook of robotics. 64–77, 1995. 537

478 Springer, 2016, pp. 1235–1266. [17] S.-O. Lee, Y.-J. Cho, M. Hwang-Bo, B.-J. You, and S.-R. Oh, 538

479 [2] S. G. Olsen and G. M. Bone, “Modelling of robotic bulldozing “A stable target-tracking control for unicycle mobile robots,” in 539

480 operations for autonomous control,” in 2011 24th Canadian Proceedings. 2000 IEEE/RSJ International Conference on Intel- 540

481 Conference on Electrical and Computer Engineering (CCECE). ligent Robots and Systems (IROS 2000)(Cat. No. 00CH37113), 541

482 IEEE, 2011, pp. 001 188–001 193. vol. 3. IEEE, 2000, pp. 1822–1827. 542

483 [3] Y.-S. Lee, S.-H. Kim, J. Seo, J. Han, and C.-S. Han, “Blade [18] J. Velagic, N. Osmic, and B. Lacevic, “Neural network con- 543

484 control in cartesian space for leveling work by bulldozer,” troller for mobile robot motion control,” World Academy of 544

485 Automation in Construction, vol. 118, p. 103264, 2020. Science, Engineering and Technology, vol. 47, pp. 193–198, 545

486 [4] F. M. Barbosa, L. B. Marcos, M. M. da Silva, M. H. Terra, 2008. 546

487 and V. G. Junior, “Robust path-following control for articulated [19] X. Zhang, Y. Fang, and N. Sun, “Visual servoing of mobile 547

488 heavy-duty vehicles,” Control Engineering Practice, vol. 85, pp. robots for posture stabilization: from theory to experiments,” 548

489 246–256, 2019. International journal of robust and nonlinear control, vol. 25, 549

490 [5] M. Natole, Y. Ying, and S. Lyu, “Stochastic proximal algorithms no. 1, pp. 1–15, 2015. 550

491 for auc maximization,” in International Conference on Machine [20] Q. Lu, D. Zhang, W. Ye, J. Fan, S. Liu, and C.-Y. Su, “Targeting 551

492 Learning. PMLR, 2018, pp. 3710–3719. posture control with dynamic obstacle avoidance of constrained 552

493 [6] G. R. Campos, P. Falcone, H. Wymeersch, R. Hult, and uncertain wheeled mobile robots including unknown skidding 553

494 J. Sjöberg, “Cooperative receding horizon conflict resolution at and slipping,” IEEE Transactions on Systems, Man, and Cyber- 554

495 traffic intersections,” in 53rd IEEE Conference on Decision and netics: Systems, 2020. 555

496 Control. IEEE, 2014, pp. 2932–2937. [21] Y. Wang, Z. Miao, H. Zhong, and Q. Pan, “Simultaneous 556

497 [7] J. Rios-Torres and A. A. Malikopoulos, “A survey on the co- stabilization and tracking of nonholonomic mobile robots: A 557

498 ordination of connected and automated vehicles at intersections lyapunov-based approach,” IEEE Transactions on Control Sys- 558

499 and merging at highway on-ramps,” IEEE Transactions on tems Technology, vol. 23, no. 4, pp. 1440–1450, 2015. 559

500 Intelligent Transportation Systems, vol. 18, no. 5, pp. 1066– [22] B. Li, Y. Fang, G. Hu, and X. Zhang, “Model-free unified 560

501 1077, 2016. tracking and regulation visual servoing of wheeled mobile 561

502 [8] J. Shi, Y. Zheng, Y. Jiang, M. Zanon, R. Hult, and B. Houskal, robots,” IEEE Transactions on Control Systems Technology, 562

503 “Distributed control algorithm for vehicle coordination at traffic vol. 24, no. 4, pp. 1328–1339, 2015. 563

504 intersections,” in 2018 European Control Conference (ECC). [23] Q. Lu, L. Yu, D. Zhang, and X. Zhang, “Simultaneous track- 564

505 IEEE, 2018, pp. 1166–1171. ing and regulation visual servoing of wheeled mobile robots 565

506 [9] M. Kneissl, A. Molin, H. Esen, and S. Hirche, “A feasible with uncalibrated extrinsic parameters,” International Journal 566

507 mpc-based negotiation algorithm for automated intersection of Systems Science, vol. 49, no. 1, pp. 217–229, 2018. 567

508 crossing,” in 2018 European Control Conference (ECC). IEEE, [24] Z. Li, J. Deng, R. Lu, Y. Xu, J. Bai, and C.-Y. Su, “Trajectory- 568

509 2018, pp. 1282–1288. tracking control of mobile robot systems incorporating neural- 569

510 [10] F. Molinari and J. Raisch, “Automation of road intersections dynamic optimized model predictive approach,” IEEE Transac- 570

511 using consensus-based auction algorithms,” in 2018 Annual tions on Systems, Man, and Cybernetics: Systems, vol. 46, no. 6, 571
 12

572 pp. 740–749, 2015. to actuator saturation: An extended state observer approach,” 641

573 [25] G. Klančar and I. Škrjanc, “Tracking-error model-based pre- Automatica, vol. 107, pp. 353–361, 2019. 642

574 dictive control for mobile robots in real time,” Robotics and [43] S. J. Yoo, “Adaptive neural tracking and obstacle avoidance of 643

575 autonomous systems, vol. 55, no. 6, pp. 460–469, 2007. uncertain mobile robots with unknown skidding and slipping,” 644

576 [26] E. Kayacan, H. Ramon, and W. Saeys, “Robust trajectory Information Sciences, vol. 238, pp. 176–189, 2013. 645

577 tracking error model-based predictive control for unmanned [44] L. Li, T. Wang, Y. Xia, and N. Zhou, “Trajectory tracking con- 646

578 ground vehicles,” IEEE/ASME Transactions on Mechatronics, trol for wheeled mobile robots based on nonlinear disturbance 647

579 vol. 21, no. 2, pp. 806–814, 2015. observer with extended kalman filter,” Journal of the Franklin 648

580 [27] W. He, S. S. Ge, Y. Li, E. Chew, and Y. S. Ng, “Neural network Institute, vol. 357, no. 13, pp. 8491–8507, 2020. 649

581 control of a rehabilitation robot by state and output feedback,” [45] B. Burns and O. Brock, “Sampling-based motion planning with 650

582 Journal of Intelligent & Robotic Systems, vol. 80, no. 1, pp. sensing uncertainty,” in Proceedings 2007 IEEE International 651

583 15–31, 2015. Conference on Robotics and Automation. IEEE, 2007, pp. 652

584 [28] J. Huang, C. Wen, W. Wang, and Z.-P. Jiang, “Adaptive output 3313–3318. 653

585 feedback tracking control of a nonholonomic mobile robot,” [46] L. Ma, J. Xue, K. Kawabata, J. Zhu, C. Ma, and N. Zheng, “Ef- 654

586 Automatica, vol. 50, no. 3, pp. 821–831, 2014. ficient sampling-based motion planning for on-road autonomous 655

587 [29] M. Hirayama, J. Guivant, J. Katupitiya, and M. Whitty, “Artif- driving,” IEEE Transactions on Intelligent Transportation Sys- 656

588 cial interlligence in path planning for autonomous bulldozers: tems, vol. 16, no. 4, pp. 1961–1976, 2015. 657

589 comparison with manual operation,” International Journal of [47] S. Liu, N. Atanasov, K. Mohta, and V. Kumar, “Search-based 658

590 Innovative Computing, Information and Control, vol. 15, no. 3, motion planning for quadrotors using linear quadratic minimum 659

591 pp. 825–844, 2019. time control,” in 2017 IEEE/RSJ international conference on 660

592 [30] S. Khan and J. Guivant, “Nonlinear model predictive path- intelligent robots and systems (IROS). IEEE, 2017, pp. 2872– 661

593 following controller for a small-scale autonomous bulldozer 2879. 662

594 for accurate placement of materials and debris of masonry [48] B. Li, Y. Zhang, Y. Ge, Z. Shao, and P. Li, “Optimal control- 663

595 in construction contexts,” IEEE Access, vol. 9, pp. 102 069– based online motion planning for cooperative lane changes of 664

596 102 080, 2021. connected and automated vehicles,” in 2017 IEEE/RSJ Inter- 665

597 [31] M. Hirayama, J. Guivant, J. Katupitiya, and M. Whitty, “System national Conference on Intelligent Robots and Systems (IROS). 666

598 and method for planning travel path for work machine, and work IEEE, 2017, pp. 3689–3694. 667

599 machine,” Jan. 30 2020, aU Patent AU2018334390B2. [49] A. J. Häusler, A. Saccon, A. P. Aguiar, J. Hauser, and A. M. 668

600 [32] X. Wang, J. Taghia, and J. Katupitiya, “Adaptive min–max Pascoal, “Energy-optimal motion planning for multiple robotic 669

601 model predictive control for field vehicle guidance in the pres- vehicles with collision avoidance,” IEEE Transactions on Con- 670

602 ence of wheel slip,” Robotics and Mechatronics for Agriculture, trol Systems Technology, vol. 24, no. 3, pp. 867–883, 2015. 671

603 pp. 157–184, 2017. [50] A. Robledo, J. Guivant, and S. Cossell, “Pseudo priority 672

604 [33] S. Wei, K. Uthaichana, M. Žefran, and R. DeCarlo, “Hybrid queues for real-time performance on dynamic programming 673

605 model predictive control for the stabilization of wheeled mobile processes applied to path planning,” in Australasian Conference 674

606 robots subject to wheel slippage,” IEEE transactions on control on Robotics and Automation, 2010. 675

607 systems technology, vol. 21, no. 6, pp. 2181–2193, 2013. [51] D. Wang and C. B. Low, “Modeling and analysis of skidding 676

608 [34] J. Taghia and J. Katupitiya, Applied Guidance Methodologies and slipping in wheeled mobile robots: Control design perspec- 677

609 for Off-road Vehicles. Springer, 2020, vol. 138. tive,” IEEE Transactions on Robotics, vol. 24, no. 3, pp. 676– 678

610 [35] S. Khan and J. Guivant, “Fast nonlinear model predictive 687, 2008. 679

611 planner and control for an unmanned ground vehicle in the [52] M. Chen, “Disturbance attenuation tracking control for wheeled 680

612 presence of disturbances and dynamic obstacles,” Scientific mobile robots with skidding and slipping,” IEEE Transactions 681

613 Reports, vol. 12, p. 12135, 2022. on Industrial Electronics, vol. 64, no. 4, pp. 3359–3368, 2016. 682

614 [36] M. W. M. Said, “Optimization based solutions for control [53] R. Fierro and F. L. Lewis, “Control of a nonholonomic mobile 683

615 and state estimation in non-holonomic mobile robots: stability, robot using neural networks,” IEEE transactions on neural 684

616 distributed control, and relative localization,” arXiv preprint networks, vol. 9, no. 4, pp. 589–600, 1998. 685

617 arXiv:1803.06928, 2018. [54] J. Huang, C. Wen, W. Wang, and Z.-P. Jiang, “Adaptive 686

618 [37] S. Khan, J. Guivant, and X. Li, “Design and experimental stabilization and tracking control of a nonholonomic mobile 687

619 validation of a robust model predictive control for the optimal robot with input saturation and disturbance,” Systems & Control 688

620 trajectory tracking of a small-scale autonomous bulldozer,” Letters, vol. 62, no. 3, pp. 234–241, 2013. 689

621 Robotics and Autonomous Systems, vol. 147, p. 103903, 2022. [55] J. C. Dunn and D. P. Bertsekas, “Efficient dynamic program- 690

622 [38] K.-S. Kim and K.-H. Rew, “Reduced order disturbance observer ming implementations of newton’s method for unconstrained 691

623 for discrete-time linear systems,” Automatica, vol. 49, no. 4, pp. optimal control problems,” Journal of Optimization Theory and 692

624 968–975, 2013. Applications, vol. 63, no. 1, pp. 23–38, 1989. 693

625 [39] R. ul Amin, I. Inayat, and L. A. Jun, “Finite time position [56] J. Nocedal and S. Wright, Numerical optimization. Springer 694

626 and heading tracking control of coaxial octorotor based on Science & Business Media, 2006. 695

627 extended inverse multi-quadratic radial basis function network [57] P. Patrinos and A. Bemporad, “Proximal newton methods for 696

628 and external disturbance observer,” Journal of the Franklin convex composite optimization,” in 52nd IEEE Conference on 697

629 Institute, vol. 356, no. 8, pp. 4240–4269, 2019. Decision and Control. IEEE, 2013, pp. 2358–2363. 698

630 [40] H.-S. Kang, Y.-T. Kim, C.-H. Hyun, and M. Park, “Generalized [58] Y. Kanayama, Y. Kimura, F. Miyazaki, and T. Noguchi, “A sta- 699

631 extended state observer approach to robust tracking control for ble tracking control method for an autonomous mobile robot,” in 700

632 wheeled mobile robot with skidding and slipping,” International Proceedings., IEEE International Conference on Robotics and 701

633 Journal of Advanced Robotic Systems, vol. 10, no. 3, p. 155, Automation. IEEE, 1990, pp. 384–389. 702

634 2013. [59] H. Yang, X. Fan, P. Shi, and C. Hua, “Nonlinear control for 703

635 [41] A. Mohammadi, M. Tavakoli, H. J. Marquez, and tracking and obstacle avoidance of a wheeled mobile robot 704

636 F. Hashemzadeh, “Nonlinear disturbance observer design with nonholonomic constraint,” IEEE Transactions on Control 705

637 for robotic manipulators,” Control Engineering Practice, Systems Technology, vol. 24, no. 2, pp. 741–746, 2015. 706

638 vol. 21, no. 3, pp. 253–267, 2013. [60] J.-J. E. Slotine and W. Li, Applied nonlinear control. Prentice 707

639 [42] Y. Yuan, Z. Wang, Y. Yu, L. Guo, and H. Yang, “Active distur- hall Englewood Cliffs, NJ, 1991, vol. 199, no. 1. 708

640 bance rejection control for a pneumatic motion platform subject

 13

709 Subhan Khan completed his Ph.D. thesis in Mecha-

710 tronic Engineering from the School of Mechani-
711 cal and Manufacturing Engineering, University of
712 New South Wales (UNSW), Sydney. He obtained
713 his BSc Computer Engineering and MSc Electronic
714 Communication and Computer Engineering degrees
715 from COMSATS Institute of Information Technol-
716 ogy (CIIT) Lahore, Pakistan and the University of
717 Nottingham, UK campus, respectively. His research
718 interests include perception, planning and model
719 predictive control of robotic systems. Currently, he
720 is working as a Postdoctoral Research Associate at the University of Sydney.

721 Jose Guivant received the B.E. degree in Electri-

722 cal Engineering from the Universidad Nacional del
723 Sur (UNS), Bahia Blanca, Argentina, and his PhD
724 degree Robotics, from The University of Sydney
725 (ACFR/USYD), Australia. He is currently Sr. Lec-
726 turer in Mechatronics at the School of Mechanical
727 Engineering, UNSW, Australia.

728