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

1
navigation task are motion planning and controlled by the speed a change on either
control. The motion planning deals with side of the robot[5].
the ability of a mobile robot to plan its
own motion in its working space, and the The dimensions of the robot .In our
motion control concerns with the ability of case we are going to need to know two
a mobile robot to follow or track a planned parameters, L is the distance between the
motion. Clearly, as mobile robot wheels and R is the radius of the wheel
applications become more complex, the shown in Figure 1, where {X, Y, Z} is the
needs for more efficient and reliable global coordinate, v is the velocity of the
motion planning and control are robot centroid, ω is the angular velocity of
indispensable[4]. The aim of mobile robotic the robot centroid, is the velocity of the
systems is the construction of autonomous
left driving wheel, is the velocity of the
systems that are capable of moving in real
right driving wheel.
environment without the comforts of
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.
Figure 1 : Kinematic Scheme of
2. KINEMATIC MODEL MOBILE differential drive of the Robot.
ROBOT
Kinematics refers to the evolution of
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 22\
driving wheels mounted on same axis with * MERGEFORMAT ()
independent actuators and one free wheel Where:
(castor). The movement of mobile robot is
done by changing the relative angular = Total velocity of the right wheels
velocities of driving wheels. The forward and backward.
assumptions are that the whole body of
robot is rigid and motion occurs without = Total velocity of the left wheels
sliding. Its wheel rotation is limited to one forward and backward.
axis. Therefore, the navigation is

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

= The change of y position.

= The change of orientation.

33\*
MERGEFORMAT ()

Where:

V= the speed. Figure 3 - 1 : Differential drive robot

= The angular velocity

[ ] {[ ] }
xr (t ) ¿ xd
3. DYMANIC Model of Mobile
yr ( t ) = ¿ yd , ∀ t 88\*
Robot
θr ( t ) ¿θ d
MODEL predictive CONTROL
MERGEFORMAT ()

[ ][ ]
System state vector in inertial frame:
x r (t) ¿ x d (t)
T =
y r (t) ¿ y d (t) 99\
x=[ ¿ x∧¿ y∧¿ θ ] 44\*
MERGEFORMAT () θr (t ) ¿ θd (t)
* MERGEFORMAT ()

[] [ ]
ẋ ( ϕ̇ r +ϕ̇ 1 ) cos ⁡θ
r
ẏ = ( ϕ̇ r + ϕ̇l ) sin ⁡θ 55\*
2
θ̇ ( ϕ̇ r− ϕ̇ l ) / D
MERGEFORMAT ()

[ ]
r
( ϕ̇ + ϕ̇ )
[]
v = 2 r 1
ω r
( ϕ̇ −ϕ̇ )
66\* Figure 3 - 2 : Block diagram of system

2D r l
MERGEFORMAT ()

3
¿ ẋ (t)=f c (x (t), u(t)) ¿ x( k +1)=f (x (k ) ,u( k2 ))
J N ( x 0 , u ) =∑ ❑ l ( xu ( k ) , u ( k ) ) ∧¿

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

‖ [ ]‖ ‖
2
2
[ ]‖
ref 2 ref 2 2
l ( x , u )=‖ xu −x ‖ +‖u−u ‖
Q R + x u (1)− 2 + u (1)− 0
0
0 Q
R
1010\*

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

uadmissible k=0
¿ subject to : x u (k +1)=f ( x u (k ) ,u(k ) ) ,
¿ x u (0)=x0 , 1313\*
¿u (k )∈ U , ∀ k ∈[0 , N−1] MERGEFO
¿ x u (k )∈ X , ∀ k ∈[0 , N ] RMAT ()

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 ] Figure 3 - 4 : Trajectory tracking
1212\*

[][ ]
MERGEFORMAT () ẋ vcos θ
ẏ = vsin θ 1414\
θ̇ ω
¿ min Φ ( w ) Objective function * MERGEFORMAT ()
w
¿ s.t. g1 (w)≤ 0 Inequality constraints ẋ 2+ ẏ 2=v 2 → v=0.5( m/ s) 1515\
¿ g2 (w)=0 Equality constraints
* MERGEFORMAT ()

ω=0( rad /s) 1616\

* MERGEFORMAT ()

[ ]
u ref = 0.5
0
1717\
* MERGEFORMAT ()

Figure 3 - 3 : Point stabilization recap

4
 nonlinear programming solvers to find a
solution[7].

x (k +1)∧¿ f ( x (k ),u(k ))+ v x

y (k )∧¿ h(x (k ))+ v y =x (k )+ v y
1919\*
MERGEFORMAT ()
Figure 3 - 5 : Trajectory tracking for N=3
2
J N ( x 0 , u )=∑ ❑l ( x u ( k ) ,u ( k ) )∧¿
k=0

‖ [ ]‖ ‖
2
0.00
[ ]‖
2
¿ xu ( 0 ) − 1 + u ( 0 )− 0.5
0
0 Q
R

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

‖ [ ]‖ ‖
2
0.50
[ ]‖
2
+ xu ( 2 )− 1 + u ( 2 )− 0.5
0
0 Q
R

1818\*
MERGEFORMAT ()
Figure 3 - 6 : State trajectory estimation
4. Moving Horizon Estimation k k−1
(MHE) JN M HE
( x ,u ) = ∑ ❑∥ ~
y ( i )−h ( x ( i ) ) ∥V +
2
∑ ❑∥ ~
u (i)
In control theory, a state observer i=k−N MHE i=k−N MHE

(estimator) is a system that provides an min J N ( x ,u)

estimate of the internal state of a given real x,u
MHE

system, from measurements of the input

s.t.: ∧x u (i+1)=f ( x u (i), u(i) )
and output of the real system. It is
¿ u(i)∈ U , ∀ i ∈ [ k −N MHE , k −1 ]2020\*
typically computer-implemented, and
provides the basis of many practical ¿ x u (i)∈ X , ∀ i∈ [ k −N MHE , k ]
applications[6].is an optimization approach MERGEFORMAT ()
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
deterministic approaches like the Kalman
filter, MHE requires an iterative approach
that relies on linear programming or
Figure 3 - 7 : Block diagram system after
added MHE

5
 ¯ k−1
x (k +1)=f ( x (k ) , u (k )) J N ( x , u )= ∑ k ❑ ∥ ~y ( i )−h ( x ( i )) ∥ 2v + ¿ ∑ ❑∥ ~
u(i)−

[ ][ ] [ ]
MHE
i=k −¿ N ¿ i =k−N MHE
x (k + 1) x (k ) v (k )cos ⁡θ(k ) MHE

y (k +1) = y (k ) + Δ T v (k )sin ⁡θ (k ) min J NMHE ( x ,u)

x ,u
θ(k +1) θ(k ) ω (k )
s.t.: ∧x u (i+1)−f ( x u (i), u(i) )=0
2121\* MERGEFORMAT () ¿ u(i)∈ U , ∀ i∈ [ k−N MHE , k −1 ]
¿ x u (i)∈ X , ∀ i ∈ [ k −N MHE , k ]

2525\* MERGEFORMAT ()

[ ]
−1
σr 0
V=
0 σα
2626\* MERGEFORMAT ()

[ ]
−1
σv 0
W= (26)
0 σω

5. Result and Simulation Discussion

Figure 3 - 8 : relative measurement sensor In this case green pointer represents the
estimation target, red pointer represents the
robot and the red line is a route of the
v( .), ω(.) robot.
2222\* MERGEFORMAT ()

¿ v (.)=v (.)+ v v  MPC robot multi shooting

¿ 4 ω( .)=ω(.)+v ω In this case also able to
2323\* MERGEFORMAT () initialize with a known guess for
v v , v ω: the state trajectory.

The red line represents the

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

[ ][ ]
robot track, and then achieved the
√ x2+ y2 vr
[]
r estimation by tracking this target.
¿ = y +
α arctan ⁡
x ()
vα

2424\* MERGEFORMAT ()

r: relative range measurement

α: relative bearing measurement

v r , vα:

6
Figure 3 - 9 : Simulation Result Robot
Multi Shooting
Figure 3 - 10 : Simulation Result Robot
Avoid

 MPC Robot avoid

It the red line represents the  MPC robot tracking
robot avoiding the barrier
represented by the blue circle, and This figures show the
then achieved the estimation by estimation tracking of the
tracking the target. robot.
Whenever the robot finds any When we increase the
barrier it can avoid it by tracking value of N the robot begin
the red line and keep following its to follow it specific track
target. earlier.

7
Figure 3 - 11 : robot tracking simulation
result
 MHE Tracking
This figures show the
resolution we can obtain
the states x and y.
This resulting state is noisy
when compared to the
ground truth data. This
method cannot be used to
obtain the robot angular
deviation (θ). And state
estimation scheme, e.g.
MHE can be used to
estimate the missing state
(θ) and improve the
estimates of states (x, y).
8
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.
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. Jadlovská. Tracking
trajectory of the mobile robot
Khepera II using approaches of
artificial intelligence. Acta
Electrotechnica et Informatica,
2011. 11(1): 38-43.
[3] Hachour, O. Path planning of
Autonomous Mobile robot.
International journal of systems
applications, engineering & localization and relative tracking
development, 2008. 2(4): 178-190.
[4] Abdalla, T.Y., M.I. Hamzah.
Trajectory tracking control for
mobile robot using wavelet
network. International Journal of
Computer Applications, 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. I Mann, and
R.G. Gosine. An optimization
based approach for relative
control in multi-robot systems.
Journal of Intelligent & Robotic
Systems, 2017. 85(2): 385-408.