Discrete Trajectory Control for Wheeled mobile

robot
End-Term Report for Mobile Robots RML7220

Manan Pathak Arbaz Khan

Roll no: M22ME006 Roll no: M22ME020
MTech, Advanced Manufacturing and Design MTech, Advanced Manufacturing and Design
Indian Institute of Technology Jodhpur Indian Institute of Technology Jodhpur
m22me006@iitj.ac.in m22me020@iitj.ac.in

Abstract—This report explains the basic idea behind the
purpose of the research for the given research paper and also
demonstrates the work done so far. In this paper the authors
have discussed a novel technique using which can successfully
apply discrete trajectory. The main contribution of this paper is
introduction of a new technique of converting discrete path into
a single continuous path. The steps followed for doing the same
has been discussed in the methodology section in detail.
Index Terms—Mobile robots, Discrete trajectory tracking,
Lyapunov stability
I. I NTRODUCTION
This article discusses about trajectory planning and control
of non-holonomic mobile robots. There are mainly two meth-
ods in which the trajectory control is divided these methods
are based upon Kinematic and Dynamic approaches which
are based on mathematical modeling. Among certain methods,
the back stepping is the proffered one because of its stability.
Though reference trajectory must satisfy essential constraint
for the stability. The article proposes a controller to solve the
problem of a discrete trajectory control for wheeled mobile
robots, which was not previously done. The algorithm has been
discussed in this report with diagrams and illustrations for the
same.
II. P ROBLEM D EFINITION
The authors mentioned that there are many kinematic meth-
ods in literature such as:
• sliding mode based approach
• input-output linearization based approaches
• fuzzy based approaches
• neural network based approaches
• back-stepping based approaches
Out of these the most applicable technique is back-stepping
technology as per the authors. Most path planners give output
in set of discrete trajectories, these are simple geometrical
shapes like line, circles, arcs, etc. These trajectories are easier
to control individually. The major contribution of this paper is
development of an algorithm that makes the path continuous.
The example of trajectory output shown by path planners is
as shown below:

Fig. 2. Encircling the discontinuity with radius ’r’

III. M ETHODOLOGY
The kinematic model considered is a simple model with
basic non-holonomic constraint as shown below:
ẏ cos θ − ẋ sin θ = 0
The posture and velocity of the robot can be given as follows:
     
ẋ ẋ cos θ 0  
v
ṗ = ẏ  = ẏ  =  sin θ 0 · = Jq (1)
Fig. 1. Discrete path as an output from path planner w
θ̇ θ̇ 0 1

Fig. 3. For condition 1a
Fig. 4. For condition 1b
A discrete trajectory is considered to as one in which the
angle is changes suddenly at a certain point. The method
replaces discontinuous points with segment micro-trajectories
(2) The second condition will clearly be:
and makes the reference quantities. The method involves
constructing a circle relevant to a [?] discontinuous point by
taking an appropriate length and ’r’ as a circle radius. The
complete methodology has 8 distinct possibilities which are
shown below:
1) First, identify a discontinuous point and create a circle of
appropriate radius ’r’.
2) Identify the point of intersection of circle with the discrete
path and label them as ’a’ and ’b’ as shown in figure
below.
3) Identify the direction of the points of intersection as the
direction of motion on the path and create two vectors at
point of intersection along the same direction
4) Identify the discontinuous point as ’o’ and make it the
centre of circle with radius ’r’.
Now we consider the vectors a and b individually to
decide an auxiliary path inside on the discontinuous break
in path. Depending upon the relative position of the
vectors ⃗a and ⃗b there are 8 different possibilities in which
different auxiliary path can be constructed.
Fig. 5. For condition 1c
Fig. 6. For condition 1d
Depending upon the angle between the two vectors these
8 categories can be sub-divided into two separate cate-
gories. The condition for the same is:
(1) The first condition is shown as follows:
|θb − θa | =
̸ kπ
This signifies that if vectors ⃗a and ⃗b are not in a
same line and their extensions will intersect at some
unique point. This condition has further four possible
orientations.
i) When the extension of ⃗a intersects with anti exten-
sion of ⃗b (refer Fig. 3 )
ii) When the extension of ⃗b intersects with anti exten-
sion of ⃗a (refer Fig. 4)
iii) When the extension of ⃗a intersects with extension
of ⃗b (refer Fig. 5)
iv) When the anti extension of ⃗a intersects with anti
extension of ⃗b (refer Fig. 6)
|θb − θa | = kπ
This shows that both vectors are aligned in the same
direction. This is further sub-divided into 4 more
conditions listed as follows:
i) When ⃗a and ⃗b are collinear and non co-incident
without offset (refer Fig. 7)
ii) When ⃗a and ⃗b are collinear and non co-incident
with offset (refer Fig. 8)
iii) When ⃗a and ⃗b are non-collinear with offset (refer
Fig. 9)
iv) When ⃗a and ⃗b are non-collinear with out offset
(refer Fig. 10)
Fig. 7. For condition 2a
Fig. 8. For condition 2b
The figures shown above and below depicts auxiliary
path for all the 8 situations discussed earlier. Such an
approach can be broken down into an algorithm that can
be programmed in Matlab for execution on paths provided
by the path planning algorithm. The path after this pre-
treatment can be then used to apply a controller. The
authors have suggested such a pre-treatment enables easy
 application on almost all the controllers. Similar to what
proposed in the paper has been tried to implement in the
results for the same can be seen in figures shown below.

Fig. 9. For condition 2c

Fig. 10. For condition 2d Fig. 12. Output for the trajectory followed by robot for given co-ordinates

VI. M ATLAB C ODE -I, P RE - TREATMENT OF PATH

IV. C ONCLUSION
In this paper the algorithm for path pre-treatment was 1 clc
majorly discussed. However, the implementation of the 2 clear all ;
same is yet to be worked upon. It is expected that the 3 p1 = [ 0 0 ;
implementation for the algorithm will be simulated by 4 20 0 ;
the end of the project. Also the implementation of a 5 28 8 ] ;
simple control law on the same may also be demonstrated 6 r1 =8;
as shown by the authors for the demonstration of the 7 n =3;
enhancement in control of the mobile robot. 8 s t e p =1;
9 f o r i =1: s t e p : n
10 X( i ) = p1 ( i , 1 )
11 Y( i ) = p1 ( i , 2 )
12 i = i +1;
13 i
14 end
15 b= s q r t ( ( X( 2 ) −X ( 1 ) ) ˆ 2 + (Y( 2 ) −Y ( 1 ) ) ˆ 2 )
16 a= s q r t ( ( X( 3 ) −X ( 2 ) ) ˆ 2 + (Y( 3 ) −Y ( 2 ) ) ˆ 2 )
17 c= s q r t ( ( X( 3 ) −X ( 1 ) ) ˆ 2 + (Y( 3 ) −Y ( 1 ) ) ˆ 2 )
18 gamma = ( ( a ˆ 2 + b ˆ2 − c ˆ 2 ) / ( 2 * a * b ) )
19 t h e t a = a c o s ( gamma )
20
21 theta deg = t h e t a *180/ pi
22 r2=r1 / tan ( t h e t a / 2 )
Fig. 11. Pre-treatment by replacing the discontinuity with a smooth path 23 i f r2 <=0
24 r 2 =− r 2 ;
25 end
V. C ONTRIBUTION 26 w deg =2 * (90 − t h e t a d e g / 2 ) ;
27 w = w deg * p i / 1 8 0 ;
Manan Pathak (M22ME006): Understanding and 28 c e n t r e x =X( 2 ) − r 2 ;
implementation of pre-treatment algorithm for converting 29 c e n t r e y =Y( 2 ) + r 1 ;
the discontinuous path to continuous curved path. 30 s t a r t a n g l e =3 * p i / 2 ;
Simulation of the same on Matlab. 31 e n d a n g l e = ( 3 * p i / 2 ) +w ;
32 f i l l e t t u r n = l i n s p a c e ( s t a r t a n g l e , end angle )
Arbaz Khan (M22ME020): Applying control law for 33 c i r c l e x = centre x + r1 * cos ( f i l l e t t u r n )
trajectory tracking for the path obtained as output after 34 c i r c l e y = centre y + r1 * sin ( f i l l e t t u r n )
pre-treatment of the path. Simulation of the same on 35
Matlab. 36
37 figure 47 x S t o r e ( i )= x ;
38 h o l d on 48 y S t o r e ( i )= y ;
39 p l o t (X, Y, ’ L i n e W i d t h ’ , 2 ) 49 t h e t a S t o r e ( i )= t h e t a ;
40 p l o t ( c i r c l e x , c i r c l e y , ’ LineWidth ’ , 2 ) 50 t i m e S t o r e ( i )= t ;
41 plot ( centre x , centre y , ’o ’ ) 51 vStore ( i )=( x dot )ˆ 2+( y dot ) ˆ 2 ;
42 g r i d on 52 i = i +1;
43 a x i s ( [ − 5 40 −5 4 0 ] ) 53
44 axis square 54 %% E u l e r ’ s update
VII. M ATLAB C ODE -II, T RAJECTORY C ONTROL 55 x = x + x dot * dt ;
56 y = y + y dot * dt ;
1 clc 57 theta = theta + theta dot * dt ;
2 clear all ; 58 end
3 tic 59
4 %% I n t i a l i z a t i o n 60 %% 2 nd p a t h
5 61 x= x S t o r e ( i − 1 ) ;
6 x =10; 62 y= y S t o r e ( i − 1 ) ;
7 y =10; 63 theta = thetaStore ( i −1);
8 t h e t a =45 * p i / 1 8 0 ; 64
9 65
10 66 xd = 1 0 ;
11 xd = 0 ; 67 yd = 0 ;
12 yd = 0 ; 68 t h e t a d =0 * p i / 1 8 0 ;
13 t h e t a d =0 * p i / 1 8 0 ; 69
14 %% C o n v e r s i o n t o p o l a r co − o r d i n a t e s 70 %% t i m e
15 % 71 t0 =0;
16 % x1= s q r t ( x ˆ 2 + y ˆ 2 ) ; 72 T=1000;
17 % x2= a t a n 2 ( y , x ) ; 73 dt =0.01;
18 % x3=x2 − t h e t a ; 74
19 %% P a r a m e t e r s 75 %% s t o r a g e v a r i a b l e
20 k1 = 0 . 5 ; 76 i =1;
21 k2 = 0 . 5 ; 77
22 78 %% Loop 2
23 %% t i m e 79 for t=t0 : dt :T
24 t0 =0; 80
25 T=1000; 81 v = −k1 * ( ( ( x−xd ) * c o s ( t h e t a ) ) +
26 dt =0.01; 82 ( ( y−yd ) * s i n ( t h e t a ) ) ) ;
27 83 i f v>=200
28 %% s t o r a g e v a r i a b l e 84 v =200;
29 i =1; 85 end
30 86 omega = −k2 * ( t h e t a − t h e t a d ) ;
31 %% Loop 1 87
32 for t=t0 : dt :T 88 x dot = v* cos ( t h e t a ) ;
33 89 y dot = v* s i n ( t h e t a ) ;
34 v = −k1 * ( ( ( x−xd ) * c o s ( t h e t a ) ) + 90 t h e t a d o t = omega ;
35 ( ( y−yd ) * s i n ( t h e t a ) ) ) ; 91
36 i f v>=200 92 %% S t o r i n g v a r i a b l e s
37 v =200; 93
38 end 94 x 2 S t o r e ( i )= x ;
39 omega = −k2 * ( t h e t a − t h e t a d ) ; 95 y 2 S t o r e ( i )= y ;
40 96 t h e t a 2 S t o r e ( i )= t h e t a ;
41 x dot = v* cos ( t h e t a ) ; 97 t i m e S t o r e ( i )= t ;
42 y dot = v* s i n ( t h e t a ) ; 98 vStore ( i )=( x dot )ˆ 2+( y dot ) ˆ 2 ;
43 t h e t a d o t = omega ; 99 i = i +1;
44 100
45 %% S t o r i n g v a r i a b l e s 101 %% E u l e r ’ s u p d a t e
46 102 x = x + x dot * dt ;
103 y = y + y dot * dt ; 159 yd = 8 ;
104 theta = theta + theta dot * dt ; 160 t h e t a d =135 * p i / 1 8 0 ;
105 end 161
106 162 %% t i m e
107 %% 3 r d p a t h 163 t0 =0;
108 x= x 2 S t o r e ( i − 1 ) ; 164 T=1000;
109 y= y 2 S t o r e ( i − 1 ) ; 165 dt =0.01;
110 theta =theta2Store ( i −1); 166
111 167 %% s t o r a g e v a r i a b l e
112 xd = 1 9 . 8 ; 168 i =1;
113 yd = 1 . 7 5 ; 169
114 t h e t a d =15 * p i / 1 8 0 ; 170 %% Loop 4
115 171 for t=t0 : dt :T
116 %% t i m e 172
117 t0 =0; 173 v = −k1 * ( ( ( x−xd ) * c o s ( t h e t a ) ) + ( ( y−yd ) * s i n ( t h e t
118 T=1000; 174 omega = −k2 * ( t h e t a − t h e t a d ) ;
119 dt =0.01; 175
120 176 x dot = v* cos ( t h e t a ) ;
121 %% s t o r a g e v a r i a b l e 177 y dot = v* s i n ( t h e t a ) ;
122 i =1; 178 t h e t a d o t = omega ;
123 179
124 %% Loop 3 180 %% S t o r i n g v a r i a b l e s
125 for t=t0 : dt :T 181
126 182 x 4 S t o r e ( i )= x ;
127 v = −k1 * ( ( ( x−xd ) * c o s ( t h e t a ) ) + 183 y 4 S t o r e ( i )= y ;
128 ( ( y−yd ) * s i n ( t h e t a ) ) ) ; 184 t h e t a 4 S t o r e ( i )= t h e t a ;
129 i f v>=200 185 t i m e S t o r e ( i )= t ;
130 v =200; 186 v4Store ( i )=( x dot )ˆ2 +( y dot ) ˆ 2 ;
131 end 187 i = i +1;
132 omega = −k2 * ( t h e t a − t h e t a d ) ; 188
133 189 %% E u l e r ’ s update
134 x dot = v* cos ( t h e t a ) ; 190 x = x + x dot * dt ;
135 y dot = v* s i n ( t h e t a ) ; 191 y = y + y dot * dt ;
136 t h e t a d o t = omega ; 192 theta = theta + theta dot * dt ;
137 193 end
138 %% S t o r i n g v a r i a b l e s 194
139 195 figure (1)
140 x 3 S t o r e ( i )= x ; 196 h o l d on
141 y 3 S t o r e ( i )= y ; 197 p l o t ( xStore , yStore , ’ LineWidth ’ , 2 ) ;
142 t h e t a 3 S t o r e ( i )= t h e t a ; 198 p l o t ( x2Store , y2Store , ’ LineWidth ’ , 2 ) ;
143 t i m e S t o r e ( i )= t ; 199 p l o t ( x3Store , y3Store , ’ LineWidth ’ , 2 ) ;
144 v3Store ( i )=( x dot )ˆ 2+( y dot ) ˆ 2 ; 200 p l o t ( x4Store , y4Store , ’ LineWidth ’ , 2 ) ;
145 i = i +1; 201 g r i d on
146 202
147 %% E u l e r ’ s update 203 figure (2)
148 x = x + x dot * dt ; 204 p l o t ( vStore , t im eSt ore , ’ LineWidth ’ , 2 )
149 y = y + y dot * dt ; 205 axis square
150 theta = theta + theta dot * dt ; 206 g r i d on
151 end 207
152 208 toc
153 %% 4 t h p a t h
154 x= x 3 S t o r e ( i − 1 ) ;
155 y= y 3 S t o r e ( i − 1 ) ;
156 theta =theta3Store ( i −1);
157
158 xd = 2 8 ;