112Equation Chapter 2 Section 1

Design and Implementation of Trajectory Tracking control of Mobile

Robot based on MHE

SalahAldeen Ibrahim Khalil Alrahman1, a, Asaad Musaab Ali Yousif1, b,

Mohamed AbdAlgayoum1, c and Wail Hamza1, d
1
karary University, College of Engineering, Sudan
a
salahibraheem123@gmail.com , b120204300022@ncepu.edu.cn ,
c
mjoe2724@gmail.com , dwailx953@gmail.com

Keywords: Trajectory tracking, mobile robot, Model predictive control, moving

horizon estimation.
Abstract:
This paper presents an efficient method of mobile robot navigation. The robot not only
helps workers reduce heavy workload, it also protects workers' physical and mental health
from the work-related-stress. However, the navigation of mobile robot in an indoor
environment is a challenging task to accomplish due to the requirement to avoid any nearby
obstacle during motion. In this paper, a design and develop of mobile robot which able to
navigate from location to another and analyze the accuracy of trajectory tracking of a mobile
robot. The experimental results and simulation showed that mobile robot able to reach the
target destination successfully without collision and obstacle, by using different types of
trajectory tracking patterns using MPC controller and MHE as feedback to optimize process
of tracking.

1. INTRODUCTION Autonomous mobile robots are being

Robots are essential elements in
society today. They are capable of
performing many tasks repetitively and
precisely without the help required by
human’s .A production line consisting of
robots may shorten the time needed to
make products and reduce the need for
humans in industry. The word robot is
used to refer to a wide variety of
mechanical machines that are capable of
movement. In order for a machine to be
classified as a robot, it must possess
artificial intelligence meaning that it must
be at least partially controlled by a
computer[1].
developed and used in many real world
applications, for example, factory
automation, underground mining, military
surveillance, and even space exploration.
In those applications, the mobile robots
often work in unknown and inhospitable
environments [2]. To survive, these robots
must be able to constantly monitor and
appropriately react to variation and
uncertainty in their environments. A basic
task that every autonomous mobile robot
must perform is safely navigating itself
from one point to another within its
environment [3]. Two key ingredients that
help the mobile robot to accomplish this
navigation task are motion planning and
control. The motion planning deals with controlled by the speed a change on either
the ability of a mobile robot to plan its side of the robot[5].
own motion in its working space, and the
motion control concerns with the ability of The dimensions of the robot .In our
a mobile robot to follow or track a planned case we are going to need to know two
motion. Clearly, as mobile robot parameters, L is the distance between the
applications become more complex, the wheels and R is the radius of the wheel
needs for more efficient and reliable shown in Figure 1,where { X , Y, Z } is the
motion planning and control are global coordinate, v is the velocity of the
indispensable[4]. The aim of mobile robotic robot centroid, ω is the angular velocity of
systems is the construction of autonomous the robot centroid, is the velocity of the
systems that are capable of moving in real
left driving wheel, is the velocity of the
environment without the comforts of
right driving wheel.
human operator by detecting objects by
means of sensors or cameras and of
processing this information into movement
without a remote control.
This desired paper is held for
information collection and analyze through
deep search for the benefits of robots,
system modeling and mathematical
calculations is achieved for design and
simulation and other applications to
implement and experiment prototypes.
Design and fabrications of printed is also
objected.

2. KINEMATIC MODEL MOBILE Figure 1 : Kinematic Scheme of

ROBOT differential drive of the
Kinematics refers to the evolution of Robot.
the position, and velocity of a mechanical
system, without referring to its mass and
inertia. The kinematic scheme of mobile
robot consists of a platform driven by two
driving wheels mounted on same axis with
independent actuators and one free wheel
(castor). The movement of mobile robot is
done by changing the relative angular 22\* MERGEFORMAT ()
velocities of driving wheels. The Where:
assumptions are that the whole body of = Total velocity of the right wheels
robot is rigid and motion occurs without forward and backward.
sliding. Its wheel rotation is limited to one
axis. Therefore, the navigation is = Total velocity of the left wheels
forward and backward.
 = The orientation. 66\* MERGEFORMAT

[ ] [ ] [ ][ ]
¿ ẋ vcos θ cos θ 0
(x, y) = The position. v
() ¿ ẏ = vsin θ = sin θ 0 77\
ω
¿ θ̇ ω 0 1
= The change of x position. * MERGEFORMAT ()

= The change of y position.

= The change of orientation.

The problem with the different

model is that it’s very cumbersome and
unnatural to think in terms of rates of
various wheels. So instead of using the
differential driver model directly, we are
going to move the unicycle model. The
unicycle model comes this issue of dealing
with unnatural or unintuitive terms, like
wheel velocities. The unicycle dynamics Figure 3 - 1 : Differential drive
are: robot

[ ] {[ ] }
xr (t ) ¿ xd
yr ( t ) = ¿ yd , ∀ t 88\*
33\* θr ( t ) ¿θ d
MERGEFORMAT () MERGEFORMAT ()

[ ][ ]
Where: x r (t) ¿ x d (t)
y r (t) = ¿ y d (t) 99\
V= the speed. θr (t ) ¿ θd (t)
= The angular velocity * MERGEFORMAT ()

3. DYMANIC Model of Mobile

Robot
T
x=[ ¿ x∧¿ y∧¿ θ ] 44\
* MERGEFORMAT ()

[] [
ẋ r ( ϕ̇ r + ϕ̇ 1 ) cos ⁡θ
ẏ = ( ϕ̇ r + ϕ̇l ) sin ⁡θ
θ̇
2
( ϕ̇ r− ϕ̇ l ) / D ] 55\* system
Figure 3 - 2 : Block diagram of

[ ]
¿ ẋ (t)=f c (x (t ), u(t)) ¿ x (k + 1)=f (x (k )

[][ ] [ ][
r
( ϕ̇ + ϕ̇ ) ẋ v cos ⁡θ x(k +1)
MERGEFORMAT ()
v = 2 r 1
ω r []
( ϕ̇ r −ϕ̇ l )
¿ ẏ = v ⋅sin ⁡θ
θ̇ ω
¿
Euler Discretization
Sampling Time ( ΔT ) y (k + 1)
θ(k +1)
=
θ
2D
 ‖ [ ]‖ ‖
Running (stage) Costs: 2
2

[
2
2
l ( x , u )=‖ xu −x ref‖Q +‖u−uref ‖R
2
J N ( x 0 , u ) =∑ ❑ l ( x u ( k), u(k) ) ∧¿ x u (0)− 2 + u(0)− 0
k=0 0
1010\* MERGEFORMAT () 0 0

‖ [ ]‖ ‖
2
2
[ ]‖
2
¿+ x u (1)− 2 + u(1)− 0
0
N −1
0 Q
R

‖ [ ]‖ ‖
¿ minimize J N ( x 0 , u ) = ∑ ❑l ( x u (k ),u( k ) ) 2
2

[ ]‖
2
+ u(2)− 0
uadmissible k=0
¿+ x u (2)− 2
¿ subject to : x u (k +1)=f ( x u (k ) ,u(k ) ) , 0
0 Q
R
¿ x u (0)=x0 ,
¿u (k )∈ U , ∀ k ∈[0 , N−1]
¿ x u (k )∈ X , ∀ k ∈[0 , N ]
1111\* MERGEFORMAT ()
N −1
¿ min J N ( x0 , u ) = ∑ ❑l ( x u (k ) ,u(k ) )
u k=0
s.t.: ∧x u (k +1)=f ( x u (k ) ,u( k ) ) ,
¿ x u (0)=x 0 ,
¿u (k )∈ U , ∀ k ∈[0 , N−1]
¿ x u (k )∈ X , ∀ k ∈[0 , N ]
1212\* MERGEFORMAT ()
Figure 3 - 4 : Trajectory tracking
¿ min Φ ( w ) Objective function

[][ ]
w
ẋ vcos θ
¿ s.t. g1 (w)≤ 0 Inequality constraints ẏ = vsin θ 1313\
¿ g2 (w)=0 Equality constraints θ̇ ω
* MERGEFORMAT ()

ẋ 2+ ẏ 2=v 2 → v=0.5( m/s) 1414\

* MERGEFORMAT ()

ω=0( rad /s) 1515\

* MERGEFORMAT ()

u = 0.5
ref

0 [ ] 1616\
* MERGEFORMAT ()
Figure 3 - 3 : Point stabilization recap
 x (k +1)∧¿ f ( x (k ),u(k ))+ v x
y (k )∧¿ h(x (k ))+ v y =x (k )+ v y
1717\* MERGEFORMAT ()

Figure 3 - 5 : Trajectory tracking for N=3

‖ [ ]‖ ‖
2
0.00
[ ]‖
2 2
J N ( x 0 , u ) =∑ ❑ l ( xu ( k ) , u ( k ) ) ∧¿ x u ( 0 )− 1 + u ( 0 )− 0.5
k=0 0
0 Q
R

‖ [ ]‖ ‖ [ ]‖
2
0.25 2
¿+ x u ( 1 )− 1 + u (1 )− 0.5
0
0 Q
R

‖ [ ]‖ ‖ [ ]‖
2
0.50 2 Figure 3 - 6 : State trajectory estimation
¿+ x u ( 2 )− 1 + u ( 2 )− 0.5
0 k k−1
0 R
Q JN M HE
( x ,u ) = ∑ ❑∥ ~
y ( i )−h ( x ( i ) ) ∥2V + ∑ ❑∥ ~
u (i)
i=k−N MHE i=k−N MHE

min J N ( x ,u)
4. Moving Horizon Estimation x,u
MHE

(MHE)
In control theory, a state observer s.t.: ∧x u (i+1)=f ( x u (i), u(i) )
(estimator) is a system that provides an ¿ u(i)∈ U , ∀ i ∈ [ k −N MHE , k −1 ]
estimate of the internal state of a given real ¿ x u (i)∈ X , ∀ i∈ [ k −N MHE , k ]
system, from measurements of the input 1818\* MERGEFORMAT ()
and output of the real system. It is
typically computer-implemented, and
provides the basis of many practical
applications[6].is an optimization approach
that uses a series of measurements
observed over time, containing noise
(random variations) and other
inaccuracies, and produces estimates of
unknown variables or parameters. Unlike Figure 3 - 7 : Block diagram system after
deterministic approaches like the Kalman added MHE
filter, MHE requires an iterative approach
that relies on linear programming or
nonlinear programming solvers to find a
solution[7].
 ¯
s.t.: ∧x u (i+1)−f ( x u (i), u(i) )=0
x (k +1)=f ( x (k ) , u (k ))

[ ][ ] [ ]
¿ u(i)∈ U , ∀ i∈ [ k−N MHE , k −1 ]
x (k + 1) x (k ) v (k )cos ⁡θ(k )
y (k +1) = y (k ) + Δ T v (k )sin ⁡θ (k ) ¿ x u (i)∈ X , ∀ i ∈ [ k −N MHE , k ]
θ(k +1) θ(k ) ω (k ) 2323\* MERGEFORMAT ()
1919\* MERGEFORMAT () MHE tuning matrices are:

v v , v ω: Additive Gaussian Noise with

standard deviations σv, and σω

[ ] [ ]
−1 −1
σr 0 σv 0
V= W=
0 σα 0 σω
2424\* MERGEFORMAT ()

5. Result and Simulation Discussion

In this case green pointer represent the
estimation target, red pointer represent the
robot and the red line is a route of the
robot.
Figure 3 - 8 : relative measurement sensor  MPC robot multi shooting
v( .), ω(.) In this case the red line
2020\* MERGEFORMAT () represent the robot avoiding the
barrier represented by the blue
¿ v (.)=v (.)+ v v
circle, then achieved the estimation
¿ ω (.)=ω(.)+ v ω
by tracking the target .
2121\* MERGEFORMAT ()

v v , v ω:

¿ y (k )=h( x (k ))+ v y

[]
¿ r =
α [
arctan ⁡
y
x
√ x2+ y2
v
+ r
vα
() ][ ]
2222\* MERGEFORMAT ()

r: relative range measurement

α: relative bearing measurement

v r , vα:
k−1

∑ k ❑ ∥ ~y (i)−h(x (i))∥ v + ∑ ❑∥ ~
2 2
J N ( x ,u)=
MHE
u (i)−u (i)∥w
i=k −¿N ¿
MHE
i=k−N MHE

min J NMHE (x , u)
x ,u
Figure 3 - 9 : Simulation Result Robot
Multi Shooting

 MPC Robot avoid

In this case the robot is moving to
the specified track then moving in
it

Figure 3 - 11 : robot tracking simulation

result

 MHE Tracking

Figure 3 - 10 : Simulation Result Robot

Avoid

 MPC robot tracking

This figures show the

estimation tracking of the r

Figure 3 - 12 : Moving Horizon Estimation

Simulation Result
 6. Conclusion
A successful implementation of a
trajectory tracking robot is illustrated in
this research. This robot does not only
have the detection capability but also the
tracking and following ability as well. The
tracking is performed on the tag and the
human is followed based on that detection.
It was also kept in mind that the following
capability of the robot should be as
efficient as possible. The tests were
performed on the different conditions to
pin point the mistakes in the algorithm and
correct them. The different sensors that
were integrated with the robot added an
additional advantage.
A wireless communication
functionality can be added to the robot to
make it more versatile and control it from
a large distance. This capability of a robot
could also be used for military purposes.
By mounting a real time video recorder on
top of the camera, we can monitor the
surroundings by just sitting in our rooms.
Reference
[1] Ouarda, H.J.I.J.o.s.a., engineering,
development. Novel mobile robot
path planning algorithm. 2010. 4(04).
[2] Šuster, P., A.J.A.E.e.I. Jadlovská.
Tracking trajectory of the mobile
robot Khepera II using approaches of
artificial intelligence. 2011. 11(1): 38-
43.
[3] Hachour, O.J.I.j.o.s.a., engineering,
development. Path planning of
Autonomous Mobile robot. 2008. 2(4):
178-190.
[4] Abdalla, T.Y., M.I.J.I.J.o.C.A. Hamzah.
Trajectory tracking control for mobile
robot using wavelet network. 2013.
74(3).
[5] Kim, W., S.-E. Lee, and B.-J. Yi.
Mobility analysis of planar mobile
robots. in Proceedings 2002 IEEE
International Conference on Robotics
and Automation (Cat. No.
02CH37292). 2002. IEEE.
[6] Mehrez, M.W., G.K. Mann, and R.G.
Gosine. Nonlinear moving horizon
state estimation for multi-robot
relative localization. in 2014 IEEE 27th
Canadian Conference on Electrical and
Computer Engineering (CCECE). 2014.
IEEE.
[7] Mehrez, M.W., G.K. Mann, R.G.J.J.o.I.
Gosine, and R. Systems. An
optimization based approach for
relative localization and relative
tracking control in multi-robot
systems. 2017. 85(2): 385-408.