Ha Noi University of Science and Technology

---- School of Electronic Engineering ----

Modeling and simulate the control system

for three-wheeled mobile robot
Instructor: A. Prof. Vu Thi Thuy Nga
Member:
Nguyen Hoang Anh – 20192183
Nguyen Thai Nguyen – 20192241
Withanage Mayomi Chandima Dulanji – 20180291
Phan Van Tinh - 20192256

1
 Table of Contents

Chapter 1: Introduction............................................................................................... 4
1.1. Wheeled Mobile Robot ............................................................................................................ 4
1.2. Several types of Wheeled Mobile Robot ................................................................................ 4
1.2.1. Differential Drive ................................................................................................................ 4
1.2.2. Bicycle Drive ....................................................................................................................... 4
1.2.3. Tricycle Drive ...................................................................................................................... 5
1.2.4. Car (Ackermann) Drive ....................................................................................................... 5
1.2.5. Omnidirectional Drive ........................................................................................................ 5
1.3. Objectives and scope of the study ........................................................................................... 5
1.4. Target implementation ............................................................................................................ 5
Chapter 2: Modeling the object of a 3-wheeled Mobile Robot ................................ 6
2.1. Kinematic model ...................................................................................................................... 6
2.1.1. Motion Constraints.............................................................................................................. 7
2.1.2. Kinematic Equation ............................................................................................................. 7
2.2. Dynamic Model ........................................................................................................................ 8
2.3. Trajectory Tracking Control ................................................................................................ 10
2.4. Trajectory Tracking Control Using Basic Approaches ...................................................... 10
2.5. Decomposing Control to Feedforward and Feedback Action............................................ 11
2.6. Feedback Linearization ......................................................................................................... 12
2.7. Development of the Kinematic Trajectory-Tracking Error Model .................................. 14
Chapter 3: Design controller for the 3-wheeled mobile robot ............................... 15
3.1. Outer loop kinematic controller design ............................................................................... 15
3.2. Design of the inner ring dynamic controller ....................................................................... 16
Chapter 4: Simulating control system on MATLAB Simulink ............................. 18
4.1. Kinematic Block ..................................................................................................................... 18
4.2. Dynamic Block ....................................................................................................................... 19
4.3. Outer Loop Kinematic Controller ........................................................................................ 20
4.4. Inner Loop Dynamic Controller ........................................................................................... 21
Chapter 5: Simulation Result and Conclusion ........................................................ 22
5.1. Simulation Result ................................................................................................................... 22
2
 5.2. Conclusion .............................................................................................................................. 23
References ................................................................................................................... 24

3
 Chapter 1: Introduction
1.1. Wheeled Mobile Robot
A mobile robot is an automatic machine that is capable of locomotion [1]. Mobile
robotics is usually considered to be a subfield of robotics and information engineering.
Mobile robots have the capability to move around in their environment and are not
fixed to one physical location. Mobile robots can be "autonomous" (AMR - autonomous
mobile robot) which means they are capable of navigating an uncontrolled environment
without the need for physical or electro-mechanical guidance devices [2]. Alternatively,
mobile robots can rely on guidance devices that allow them to travel a pre-defined
navigation route in relatively controlled space [3]. By contrast, industrial robots are
usually more-or-less stationary, consisting of a jointed arm (multi-linked manipulator)
and gripper assembly (or end effector), attached to a fixed surface.
1.2. Several types of Wheeled Mobile Robot
1.2.1. Differential Drive
Differential drive is a very simple driving
mechanism that is quite often used in practice, especially
for smaller mobile robots. Robots with this drive usually
have one or more castor wheels to support the vehicle
and prevent tilting. Both main wheels are placed on a
common axis. The velocity of each wheel is controlled
by a separate motor.
1.2.2. Bicycle Drive
Unlike Differential Dive, Bicycle Drive, although
also has 2 wheels, but its 2 wheels are arranged in a
straight line, and usually only one wheel is active and one
wheel can control the steering angle, similar in structure
like a bicycle. This type of robot is very rare due to its
low practical application.

4
 1.2.3. Tricycle Drive
Tricycle Drive is a combination of the 2 types of WMR
mentioned above, it has 3 wheels and in which 2 rear wheels
are arranged coaxially, and one front wheel acts as a steering
wheel. Two of the 3 wheels will be attached to the actuator
for control and the other wheel is free or can also use the
actuator to control only one front rudder at the desired speed
and steering angle.
1.2.4. Car (Ackermann) Drive
Cars use the Ackermann steering principle. The
idea behind the Ackerman steering is that the inner
wheel (closer to ICR) should steer for a bigger angle
than the outer wheel in order to allow the vehicle to
rotate around the middle point between the rear
wheel axes. Consequently, the inner wheel travels
with a slower speed than the outer wheel.

1.2.5. Omnidirectional Drive

As a specially designed robot with the axis of the biscuits at one point, this feature
will help the robot move in all directions and reach every point.
1.3. Objectives and scope of the study
For this project, we chose the Tricycle Drive robot model with 2 rear wheels as
active wheels attached with actuators to control and front wheels as free wheels.
1.4. Target implementation
Design a 2-loop controller to control the robot to follow a given trajectory.
Simulating and checking the robot model and the efficiency of the robot in MATLAB
Simulink.

5
 Chapter 2: Modeling the object of a 3-wheeled Mobile Robot

2.1. Kinematic model

Where:
ICR: instantaneous center of rotation
R(t): instantaneous radius of the vehicle driving trajectory
ω: angular velocity
v: forward velocity
r: radius of robot wheel
α: steering wheel angle of the vehicle relative to 𝑂𝑚𝑋𝑚 dimension
The vehicle model is placed in a general coordinate system (𝑋𝑔 , 𝑌𝑔 ) and the
coordinate system of motion associated with the vehicle (𝑋𝑚 , 𝑌𝑚 ). The robot pose in a
plane is defined by its state vector
𝑇
𝑞̇ = [ 𝑥̇ 0 𝑦̇ 0 𝜑̇ 𝜃̇𝑟 𝜃̇𝑙 ] ( 2.1.1)
Since the movement of the vehicle is controlled by the transmission of the rear
wheels, thereby adjusting the direction of the vehicle, the forward velocity and the
angular velocity are determined respectively by:

6
 𝑉𝑟 − 𝑉𝑙
𝜔(𝑡) =
𝐿
{ 𝑉𝑟 −𝑉𝐿 ( 2.1.2)
𝑣(𝑡) =
2

2.1.1. Motion Constraints

In a kinematic model, assuming that in each contact there exist a pure rolling motion
without slipping, the following constraints are obtained:
𝑦̇ 0 𝑐𝑜𝑠𝜑 − 𝑥̇ 0 𝑠𝑖𝑛𝜑 = 0
{𝑥̇ 0 𝑐𝑜𝑠 𝜑 + 𝑦̇ 0 𝑠𝑖𝑛𝜑 + 𝑏𝜑̇ − 𝑟𝜃̇𝑟 = 0 ( 2.1.3)
𝑥̇ 0 𝑐𝑜𝑠 𝜑 + 𝑦̇ 0 𝑠𝑖𝑛𝜑 − 𝑏𝜑̇ − 𝑟𝜃̇𝑙 = 0
Where 𝜃𝑟 and 𝜃𝑙 are the angular displacement of the right and left wheels
respectively, 𝑥0 and 𝑦0 denoted the actual position of 𝑃0 , 𝜙 is the heading angle of the
platform.
2.1.2. Kinematic Equation
These three constraints given by (1)-(3) are expressed as:

𝐴(𝑞). 𝑞̇ = 0 ( 2.1.4 )
Where:
−𝑠𝑖𝑛𝜑 𝑐𝑜𝑠𝜑 0 0 0
𝐴(𝑞) = ⌊ 𝑐𝑜𝑠𝜑 𝑠𝑖𝑛𝜑 𝑏 −𝑟 0⌋ ( 2.1.5)
−𝑐𝑜𝑠𝜑 −𝑠𝑖𝑛𝜑 −𝑏 0 −𝑟
𝑇
𝑞̇ = [ 𝑥̇ 0 𝑦̇ 0 𝜑̇ 𝜃̇𝑟 𝜃̇𝑙 ] ( 2.1.6)
Robot external kinematics (in global coordinates) is given by:
𝑐𝑜𝑠𝜑 0
𝑠𝑖𝑛𝜑 0
0 1 𝑣
𝑞̇ = 𝑆(𝑞)𝑣(𝑡) = 1 𝑏 [ ] ( 2.1.7)
𝜔
𝑟 𝑟
1 −𝑏
[ 𝑟 𝑟 ]
𝑆(𝑞) matrix has the following relation with 𝐴(𝑞) matrix:

𝐴(𝑞)𝑆(𝑞) = 0 ( 2.1.8)

7
Therefore, two basis motion direction 𝑆1 , and 𝑆2 :

𝑐𝑜𝑠𝜑 0
𝑠𝑖𝑛𝜑 0
0 1
𝑆1 = 1 , 𝑆2 = 𝑏 ( 2.1.9)
𝑟 𝑟
1 −𝑏
[ 𝑟 ] [𝑟]
2.2. Dynamic Model
The 3-wheeler model under consideration is a model with 2 rear wheels as active
wheels controlled by 2 engines, and front wheels as free wheels. These two actuators
will generate 𝜏𝑟 , 𝜏𝑙 respectively.
According to the Euler-Lagrange formulation, the nonholonomic wheeled mobile
robot model can be described as:

𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝐺(𝑞) = 𝐵(𝑞)𝜏 − 𝐴𝑇 (𝑞)𝜆 ( 2.2.1)

Where:
𝑀(𝑞) 𝑖𝑠 𝑎 𝑠𝑦𝑚𝑚𝑒𝑡𝑟𝑖𝑐 𝑝𝑜𝑠𝑖𝑡𝑖𝑣𝑒 𝑑𝑒𝑓𝑖𝑛𝑖𝑡𝑒 𝑖𝑛𝑒𝑟𝑡𝑖𝑎 𝑚𝑎𝑡𝑟𝑖𝑥 𝑜𝑓 𝑡ℎ𝑒 𝑠𝑦𝑠𝑡𝑒𝑚.
𝐶(𝑞, 𝑞̇ ) 𝑖𝑠 𝑎 𝑐𝑒𝑛𝑡𝑟𝑖𝑝𝑒𝑡𝑎𝑙 𝐶𝑜𝑟𝑖𝑜𝑙𝑖𝑠 𝑚𝑎𝑡𝑟𝑖𝑥.
𝐺(𝑞) 𝑖𝑠 𝑡ℎ𝑒 𝑔𝑟𝑎𝑣𝑖𝑡𝑎𝑡𝑖𝑜𝑛𝑎𝑙 𝑣𝑒𝑐𝑡𝑜𝑟.
𝐵(𝑞) 𝑖𝑠 𝑡ℎ𝑒 𝑖𝑛𝑝𝑢𝑡 𝑡𝑟𝑎𝑛𝑠𝑓𝑜𝑟𝑚𝑎𝑡𝑖𝑜𝑛 𝑚𝑎𝑡𝑟𝑖𝑥.
𝜏 𝑖𝑠 𝑎 𝑐𝑜𝑛𝑡𝑟𝑜𝑙 𝑖𝑛𝑝𝑢𝑡 𝑣𝑒𝑐𝑡𝑜𝑟.
𝐴 𝑖𝑠 𝑎 𝑚𝑎𝑡𝑟𝑖𝑥 𝑎𝑠𝑠𝑜𝑐𝑖𝑎𝑡𝑒𝑑 𝑤𝑖𝑡ℎ 𝑛𝑜𝑛ℎ𝑜𝑙𝑜𝑛𝑜𝑚𝑖𝑐 𝑐𝑜𝑛𝑠𝑡𝑟𝑎𝑖𝑛𝑡𝑠.
𝜆 𝑖𝑠 𝑡ℎ𝑒 𝑣𝑒𝑐𝑡𝑜𝑟 𝑜𝑓 𝐿𝑎𝑔𝑟𝑎𝑛𝑔𝑒 𝑚𝑢𝑙𝑡𝑖𝑝𝑙𝑖𝑒𝑟𝑠.
𝑞̈ 𝑎𝑛𝑑 𝑞̇ 𝑑𝑒𝑛𝑜𝑡𝑒 𝑣𝑒𝑙𝑜𝑐𝑖𝑡𝑦 𝑎𝑛𝑑 𝑎𝑐𝑐𝑒𝑙𝑒𝑟𝑎𝑡𝑖𝑜𝑛 𝑣𝑒𝑐𝑡𝑜𝑟𝑠.
On the other hand, the derivative on both sides of 2.7 we have: 𝑞̈ = 𝑆̇𝑣 + 𝑆𝑣̇ .
Equation 2.17 becomes:
̇ 𝐺 + ̇ 𝜏 = 𝐸. 𝑢 − 𝐴𝑇 . 𝜆
𝑀. 𝑆. 𝑣 + 𝑀. 𝑆. 𝑣 + 𝑉 + 𝐹 + ( 2.2.2)
𝑑
Multiplying equation with 𝑆 𝑇 , we have:

𝑆 𝑇 . 𝑀. 𝑆̇. 𝑣 + 𝑆 𝑇 . 𝑀. 𝑆. 𝑣̇ + 𝑆 𝑇 (𝑉 + 𝐹 + 𝐺 + 𝜏𝑑 ) = 𝑆 𝑇 . 𝐸. 𝑢 ( 2.2.3)
Suppose:

8
 𝑆 𝑇 . 𝑀. 𝑆̇. 𝑣 + 𝑆 𝑇 (𝑉 + 𝐹 + 𝐺 + 𝜏𝑑 ) = 𝑉
{ 𝑆 𝑇 . 𝑀. 𝑆 = 𝑀 ( 2.2.4)
𝑆𝑇 . 𝐸 = 𝐸
Equation 2.19 becomes:
̇ 𝐸. 𝑢
𝑀. 𝑣 + 𝑉 = ( 2.2.5)
Therefore:

𝑣̇ = 𝑀−1 . (𝐸𝑢 − 𝑉) ( 2.2.6)

By extending the state vector with velocities 𝑥 = [𝑞 𝑇 𝑣 𝑇 ]𝑇 and presenting the
system in nonlinear form 𝑥̇ = 𝑓(𝑥) + 𝑔(𝑥) (expression 𝑓(𝑥) contains nonlinear
dependence from states) the state-space representation of the system reads.
𝑆𝑣 0
𝑥̇ = [ ̃ −1 ̃ ] + [ 𝑛×𝑟 ]𝑢 ( 2.2.7)
−𝑀 𝑉 −𝑀̃ −1 𝐸̃
Where matrices are:
𝑐𝑜𝑠𝜑 𝑐𝑜𝑠𝜑
𝑚 0 0
1 𝑠𝑖𝑛𝜑 𝑠𝑖𝑛𝜑 ]
𝑀 = [0 𝑚 0] 𝐸 =
𝑟
[
𝐿 −𝐿
( 2.2.8)
0 0 𝐽
2 2

−𝑠𝑖𝑛𝜑 𝑐𝑜𝑠𝜑 0 0 0
𝐴(𝑞) = ⌊ 𝑐𝑜𝑠𝜑 𝑠𝑖𝑛𝜑 𝑏 −𝑟 0⌋ ( 2.2.9)
−𝑐𝑜𝑠𝜑 −𝑠𝑖𝑛𝜑 −𝑏 0 −𝑟
𝜏𝑟
𝑢 = [𝜏 ] ( 2.2.10)
𝑙
The common state-space model (2.57) that includes the kinematic and dynamic
model is determined by matrices:

̃ = [𝑚
𝑀
0 0
] , 𝑉̃ = [ ] ( 2.2.11)
0 𝐽 0

1
11
𝐸̃ = [ 𝐿 −𝐿 ] ( 2.2.12)
𝑟
2 2
and according to Eq. (2.57) the system can be written in the state-space form
𝑥 = 𝑓(𝑥) + 𝑔(𝑥)𝑢, where the state vector is 𝑥 = [𝑞 𝑇 , 𝑣 𝑇 ]𝑇 . The resulting model is:

9
 0 0
𝑥̇ 𝑣𝑐𝑜𝑠𝜑
0 0
𝑦̇ 𝑣𝑠𝑖𝑛𝜑 0 0 𝜏𝑟
𝜑̇ = 𝜔 + 1 1 [𝜏 ] ( 2.2.13)
𝑙
𝑣̇ 0 𝑚𝑟 𝑚𝑟
[𝜔̇ ] [ 0 ] 𝐿 −𝐿
[2𝐽𝑟 −2𝐽𝑟 ]

2.3. Trajectory Tracking Control

In mobile robotics, a path is a “line” that a robot needs to drive in the generalized
coordinates space. If a path is parameterized with time, that is, motion along the path
has to be synchronized with time, then we speak about a trajectory. Whenever the
schedule of a robot motion is known in advance, the reference trajectory of the robot
can be written as a time function in the generalized coordinate’s space:

𝑞𝑟𝑒𝑓 (𝑡) = [𝑥𝑟𝑒𝑓 (𝑡) 𝑦𝑟𝑒𝑓 (𝑡) 𝜑𝑟𝑒𝑓 ] 𝑇 ( 2.3.1)

For practical reasons, the trajectory is always defined on a finite time interval 𝑡 ∈
[0 𝑇], that is, the reference trajectory has a start point and an end point. Trajectory
tracking control is a mechanism that will ensure the robot trajectory 𝑞(𝑡) is as close as
possible to the reference one 𝑞𝑟𝑒𝑓 (𝑡) despite any difficulties encountered.
2.4. Trajectory Tracking Control Using Basic Approaches
When thinking of implementing a trajectory tracking control, the first idea would
probably be to imagine the reference trajectory as a moving reference position. Then in
each sampling time of the controller, the reference point moves to the current point of
the reference trajectory (𝑥𝑟𝑒𝑓 (𝑡), 𝑦𝑟𝑒𝑓 (𝑡) ), and the control to the reference position is
applied via control laws. Note that here the robot is required to drive as closely as
possible to this imaginary reference point. When the velocity is low and the position
measurement is noisy, the robot position measurement can be found ahead of the
trajectory. It is therefore extremely important to handle such situations properly, for
example, by using an updated control law.
This approach suffers from the fact that feedback carries the main burden here and
relatively large control gains are needed in order to make the control errors small. This
makes the approach susceptible to disturbances in the control loop. It is therefore
advantageous to use feedforward compensation that will be introduced in Section 2.5.

10
2.5. Decomposing Control to Feedforward and Feedback Action

The approach described in section 2.4 is easy to implement, but it has the
disadvantage that it is very susceptible to noise in the control loops, so a feedforward
noise compensation component is required.
Before introducing feedforward and feedback control, we need to define another
important system property. A system is called differentially flat if there exist a set of
flat outputs, and all the system states and system inputs can be rewritten as functions of
these flat outputs and a finite number of their time derivatives. This means that some
nonlinear functions 𝐹𝑥 and 𝐹𝑢 have to exist satisfying:
𝑑𝑝
𝑥 = 𝑓𝑥 (𝑧𝑓 , 𝑧̇𝑓 , 𝑧̈𝑓 , … , 𝑧 ) ( 2.5.1)
𝑑𝑡 𝑝 𝑓

𝑑𝑝
𝑢 = 𝑓𝑢 (𝑧𝑓 , 𝑧̇𝑓 , 𝑧̈𝑓 , … , 𝑧 ) ( 2.5.2)
𝑑𝑡 𝑝 𝑓
Where vectors 𝑥, 𝑢, and 𝑧𝑓 represent the system states, its inputs, and flat outputs,
respectively, while 𝑝 is a finite integer. We should also mention that flat outputs should
be functions of system states, its inputs, and a finite number of input derivatives.
In case of the differential drive kinematic model given by Eq. (2.1.7) the flat outputs
are actual system outputs 𝑥 and 𝑦. It can easily be shown that all the inputs (both
velocities) and the third state (robot orientation) can be represented as functions of 𝑥
and 𝑦, and their derivatives.
Therefore, the translational velocity and tangent of the orientation are obtained by
the Pythagorean sum of its component:

11
 𝑣(𝑡) = √𝑥̇ 2 + 𝑦̇ 2
{ 𝑦̇ (𝑡) ( 2.5.3)
𝜑(𝑡) = 𝑡𝑎𝑛−1 ( )
𝑥̇ (𝑡)

Finally, angular velocity 𝜔(𝑡) is determined as the time derivative of the orientation
𝜑(𝑡) given by Eq. (2.5.3)
𝑑 𝑦̇ (𝑡) 𝑥̇ (𝑡)𝑦̈ (𝑡)− 𝑦̇ (𝑡)𝑥̈ (𝑡)
𝜔(𝑡) = [𝑡𝑎𝑛−1 ( )] = ( 2.5.4)
𝑑𝑡 𝑥̇ (𝑡) 𝑥̇ 2 (𝑡)+ 𝑦̇ 2 (𝑡)

In the case of a differentially driven wheeled robot, equation (2.5.3), Wheeled

Mobile Robotics (2.5.4) provide formulae for obtaining reference velocities 𝑣𝑟𝑒𝑓 (𝑡)
and 𝜔𝑟𝑒𝑓 (𝑡) from the reference trajectory given by 𝑥𝑟𝑒𝑓 (𝑡) and 𝑦𝑟𝑒𝑓 (𝑡):

𝑣𝑟𝑒𝑓 (𝑡) = √𝑥̇ 𝑟𝑒𝑓 2 + 𝑦̇ 𝑟𝑒𝑓 2

{ ( 2.5.5)
𝑥̇ 𝑟𝑒𝑓 (𝑡)𝑦̈ 𝑟𝑒𝑓 (𝑡)− 𝑦̇ 𝑟𝑒𝑓 (𝑡)𝑥̈ 𝑟𝑒𝑓 (𝑡)
𝜔𝑟𝑒𝑓 =
𝑥̇ 𝑟𝑒𝑓 2 (𝑡)+ 𝑦̇ 𝑟𝑒𝑓 2 (𝑡)

2.6. Feedback Linearization

The idea of feedback linearization is to introduce some transformation (usually to
the system input) that makes the system between new input and output linear. Thus,
any linear control design is made possible. First, we have to make sure that the system
is differentially flat [6, 7]. We have shown in Section 2.5 that many kinematic structures
are flat. Then, the procedure of designing a feedback linearization is as follows:
• The appropriate flat outputs should be chosen. Their number is the same as the
number of the system inputs.
• Flat outputs should be differentiated, and the obtained derivatives checked for
the functional presence of inputs. This step is repeated until all the inputs (or their
derivatives) appear in the flat output derivatives. If all the inputs (more precisely
their highest derivatives) can be derived from this system of equations, we can
proceed to the next step.
• The system of equations is solved for the highest derivatives of individual inputs.
To obtain actual system inputs, a chain of integrators has to be used on the input
derivatives. The output derivatives, on the other hand, serve as new inputs to the
system.
• Since the newly obtained system is linear, a wide selection of possible control
laws can be used on these new inputs

12
 In the case of a wheeled mobile robot with differential drive, the flat outputs are
𝑥(𝑡) and 𝑦(𝑡). Their first derivative according to the kinematic model (2.1.7) is:
𝑥̇ = 𝑣𝑐𝑜𝑠𝜑
{ ( 2.6.1)
𝑦̇ = 𝑣𝑠𝑖𝑛𝜑
In the first derivatives, only the translational velocity 𝑣 appears, and the
differentiation continues:
𝑥̈ = 𝑣̇ 𝑐𝑜𝑠𝜑 + 𝑣𝑠𝑖𝑛𝜑
{ ( 2.6.2)
𝑦̈ = 𝑣̇ 𝑠𝑖𝑛𝜑 + 𝑣𝑐𝑜𝑠𝜑
In the second derivatives both velocities (𝑣 and 𝜔 = 𝜑̇ ) are present. Now the system
of equations is rewritten so that the second derivatives of the flat outputs are described
as functions of the highest derivatives of individual inputs (𝑣̇ and 𝜔 in this case):
𝑥̈ 𝑐𝑜𝑠𝜑 𝑠𝑖𝑛𝜑 𝑣̇ 𝑣̇
[ ]= [ ][ ] = 𝐹[ ] ( 2.6.3)
𝑦̈ 𝑠𝑖𝑛𝜑 𝑣𝑐𝑜𝑠𝜑 𝜔 𝜔
The matrix 𝐹 has been introduced, which is nonsingular if 𝑣 ≠ 0. The system of
equations can therefore be solved for 𝑣 and 𝜔:

𝑣̇ 𝑥̈ 𝑐𝑜𝑠𝜑 𝑠𝑖𝑛𝜑 𝑥̈
[ ] = 𝐹 −1 [ ] = [−𝑠𝑖𝑛𝜑 𝑐𝑜𝑠𝜑 ] [ ] ( 2.6.4)
𝜔 𝑦̈ 𝑦̈
𝑣 𝑣
The solution 𝜔 from Eq. (3.25) is the actual input to the robot, while the solution
𝑣̇ should be integrated before it can be used as an input. The newly obtained linear
system has inputs [𝑢1 , 𝑢2 ] 𝑇 = [𝑥̈ , 𝑦̈ ] 𝑇 , and the states 𝑧 = [𝑥, 𝑦, 𝑥̇ , 𝑦̇ ] 𝑇 (the kinematic
model (2.1.7) has three states; the fourth one is due to an additional integrator). The
dynamics of the new system can be described conveniently by the state-space
representation:
𝑥̇ 0 1 0 0 𝑥 0 0
𝑥̈
[𝑦̇ ] = [0 0 0 0] [𝑥̇ ] + [1 0] [𝑢1 ] ( 2.6.5)
0 0 0 1 𝑦 0 0 𝑢2
𝑦̈ 0 0 0 0 𝑦̇ 0 1
or in a compact form as:

𝑧̇ = 𝐴𝑧 + 𝐵𝑢 ( 2.6.6)
The system (2.6.6) is controllable because its controllability matrix, has full rank,
and therefore the state controller exists for an arbitrarily chosen characteristic
polynomial of the closed loop.

13
 𝑄𝑐 = [𝐵, 𝐴𝐵] ( 2.6.7)
Eq. (2.6.6) can also be written for the reference signals:

𝑧̇𝑟𝑒𝑓 = 𝐴𝑧𝑟𝑒𝑓 + 𝐵𝑢𝑟𝑒𝑓 ( 2.6.8)

Where 𝑢𝑟𝑒𝑓 = [𝑥𝑟𝑒𝑓 𝑥̇ 𝑟𝑒𝑓 𝑦𝑟𝑒𝑓 𝑦̇ 𝑟𝑒𝑓 ], and 𝑢𝑟𝑒𝑓 = [𝑥̈ 𝑟𝑒𝑓 𝑦̈ 𝑟𝑒𝑓 ] ( 2.6.9)
The error between the actual states and the reference states is defined as 𝑧̃ = 𝑧 − 𝑧𝑟𝑒𝑓 .

Subtracting Eq. (2.6.8) from Eq. (2.6.6) yields:

𝑧̃̇ = 𝐴𝑧̃ + 𝐵(𝑢 − 𝑢𝑟𝑒𝑓 ) ( 2.6.10)

2.7. Development of the Kinematic Trajectory-Tracking Error Model

The posture error 𝑒(𝑡) = [𝑥𝑟𝑒𝑓 𝑦𝑟𝑒𝑓 𝜑𝑟𝑒𝑓 ] 𝑇 is determined using the actual
posture 𝑞(𝑡) = [𝑥(𝑡) 𝑦(𝑡) 𝜑(𝑡)] 𝑇 of the real robot and the reference posture
𝑞𝑟𝑒𝑓 (𝑡) = [𝑥𝑟𝑒𝑓 (𝑡) 𝑦𝑟𝑒𝑓 (𝑡) 𝜑𝑟𝑒𝑓 (𝑡)] 𝑇 of the virtual reference robot:

𝑒𝑥 (𝑡) 𝑐𝑜𝑠(𝜑(𝑡)) 𝑠𝑖𝑛(𝜑(𝑡)) 0

[ 𝑒𝑦 (𝑡) ] = [− 𝑠𝑖𝑛(𝜑(𝑡)) 𝑐𝑜𝑠(𝜑(𝑡)) 0] (𝑞𝑟𝑒𝑓 (𝑡) − 𝑞(𝑡)) ( 2.7.1)
𝑒𝜑 (𝑡) 0 0 1
Assuming that the actual and the reference robot have the same kinematic model
given by Eq. (2.1.6) and taking into account the transformation (2.7.1), the posture error
model can be written as follows:

14
 𝑒̇𝑥 𝑐𝑜𝑠𝑒𝜑 0 𝑣 −1 𝑒𝑦
𝑟𝑒𝑓
𝑒̇
[ 𝑦 ] = [ 𝑠𝑖𝑛𝑒𝜑 0] [𝜔 ] + [ 0 −𝑒𝑥 ] ( 2.7.2)
𝑟𝑒𝑓
𝑒̇𝜑 0 1 0 −1
The control 𝑢 is decomposed as:

𝑣 𝑣𝑟𝑒𝑓 𝑐𝑜𝑠 (𝑒𝜑 ) + 𝑣𝑓𝑏

𝑢= [ ]= [ ] ( 2.7.3)
𝜔 𝜔𝑟𝑒𝑓 + 𝜔𝑓𝑏
Where 𝑣𝑓𝑏 and 𝜔𝑓𝑏 are the feedback signals to be defined later, while 𝑣𝑟𝑒𝑓 𝑐𝑜𝑠 (𝑒𝜑 )
and 𝜔𝑟𝑒𝑓 are the feedforward signals, although technically speaking, 𝑣𝑟𝑒𝑓 𝑐𝑜𝑠(𝑒𝜑 ) is
modulated with the orientation error that originates from the output. On the other hand,
when the orientation error is driven to 0, 𝑣𝑟𝑒𝑓 𝑐𝑜𝑠 (𝑒𝜑 ) becomes a “true” feedforward.
Inserting the control (2.7.1) into Eq. (2.7.2) results in the tracking-error model:
𝑒̇𝑥 = 𝜔𝑟𝑒𝑓 𝑒𝑦 − 𝑣𝑓𝑏 + 𝑒𝑦 𝜔𝑓𝑏
{𝑒̇𝑦 = −𝜔𝑟𝑒𝑓 𝑒𝑥 − 𝑣𝑟𝑒𝑓 𝑠𝑖𝑛(𝑒𝜑 ) + 𝑒𝑥 𝜔𝑓𝑏 ( 2.7.4)
𝑒̇𝜑 = −𝜔𝑓𝑏
The control goal is to drive the errors of the error model (2.7.4) toward 0 by
appropriately choosing the controls 𝑣𝑓𝑏 and 𝜔𝑓𝑏 . This is the topic of the following
sections.

Chapter 3: Design controller for the 3-wheeled mobile robot

3.1. Outer loop kinematic controller design
The error model (2.7.4) is nonlinear. In this section this model will be linearized to
enable the use of a linear controller. The linearization has to take place around some
equilibrium point. Here the obvious choice is the zero error (𝑒𝑥 = 𝑒𝑦 = 𝑒𝜑 = 0). This
point is an equilibrium point of Eq. (2.7.4) if both feedback velocities are also 0 (𝑣𝑓𝑏 =
0, 𝜔𝑓𝑏 = 0). Linearizing Eq. (2.7.4) around the zero-error yields the following:

𝑒̇𝑥 0 𝜔𝑟𝑒𝑓 0 −1 0 𝑣
𝑣𝑟𝑒𝑓 𝑓𝑏
[ 𝑒̇𝑦 ] = [−𝜔𝑟𝑒𝑓 0 𝑣𝑟𝑒𝑓 ] [𝜔 ] + [ 0 0 ] [𝜔 ] ( 3.1.1)
𝑟𝑒𝑓 𝑓𝑏
𝑒̇𝜑 0 0 0 0 −1
which is a linear time-varying system due to 𝑣𝑟𝑒𝑓 and 𝜔𝑟𝑒𝑓 being time dependent.
The system given by Eq. (3.1.1) is a state-space representation of a dynamic error-
system where all the states (errors in this case) are accessible. A full state feedback is

15
therefore possible if the system is controllable. If it is assumed that 𝑣𝑟𝑒𝑓 and 𝜔𝑟𝑒𝑓 are
constant (the reference path consists of straight-line segments and circular arcs), it is
easy to confirm that the controllability matrix defined by Eq. (2.6.7) is of the full rank,
and consequently all the errors can be forced to 0 by a static state feedback.
Due to a special structure of the system (3.1.1), a static state feedback with a simple
form of gain matrix is often used:
𝑒𝑥
𝑣𝑓𝑏 𝑘𝑥 0 0
[𝜔 ] = [ 0 𝑒𝑦 ( 3.1.2)
𝑓𝑏 𝑘𝑦 𝑘 𝜑 ] [𝑒 ]
𝜑

We see that the error in the direction of driving is corrected by 𝑣𝑓𝑏 , while the errors
in the orientation and in the lateral directions are corrected by 𝜔𝑓𝑏 .
We choose the controller:
𝑣𝑐 𝑣𝑟𝑒𝑓 𝑐𝑜𝑠 (𝑒𝜑 ) + 𝑣𝑓𝑏
[𝜔 ] = [ ] ( 3.1.3)
𝑐 𝜔𝑟𝑒𝑓 + 𝜔𝑓𝑏
𝑒𝑥
𝑣𝑓𝑏 𝑘𝑥 0 0
Where: [𝜔 ] = [ 0 𝑘 ] [ 𝑒𝑦 ] ( 3.1.4)
𝑓𝑏 𝑦 𝑘𝜑 𝑒
𝜑

𝑘𝑥 = 𝑘𝜑 = 2𝜁 𝜔𝑛 = 2𝜁√𝜔 2 𝑟𝑒𝑓 + 𝑔𝑣 2 𝑟𝑒𝑓

And { ( 3.1.5)
𝑘𝑦 = 𝑔𝑣𝑟𝑒𝑓

𝑔 = 85
Choose: { ( 3.1.6)
𝜁 = 0.9
3.2. Design of the inner ring dynamic controller
Control target:
𝑣 𝑣𝑐 0
([ ] − [𝜔 ]) → [ ] 𝑘ℎ𝑖 𝑡 → ∞ ( 3.2.1)
𝜔 𝑐 0
We have:
𝜏
𝑣̇ ̃ −1 (𝐸̃ . [ 𝑅 ] − 𝑉̃ . [ 𝑣 ])
[ ]= 𝑀 ( 3.2.2)
𝜔̇ 𝜏𝐿 𝜔
And:

16
 𝑚 0 0 𝑐𝑜𝑠𝜑 0
̃ = 𝑆 . 𝑀. 𝑆 = [𝑐𝑜𝑠𝜑
𝑇 𝑠𝑖𝑛𝜑 0 𝑚 0
𝑀 ][0 𝑚 0] [ 𝑠𝑖𝑛𝜑 0] = [ ]
0 0 1 0 0 𝐽
0 𝐽 0 1
𝑐𝑜𝑠𝜑 𝑐𝑜𝑠𝜑
𝑐𝑜𝑠𝜑 𝑠𝑖𝑛𝜑 0 1 𝑠𝑖𝑛𝜑 𝑠𝑖𝑛𝜑 1 1 1
𝐸̃ = 𝑆 𝑇 . 𝐸 = [ ]. .[ 𝐿 ] = . [ 𝐿 −𝐿] ( 3.2.3)
0 0 1 𝑟 −𝐿 𝑟
2 2
2 2
𝑚 0 0 −𝜑̇ 𝑐𝑜𝑠𝜑 0
𝑐𝑜𝑠𝜑 𝑠𝑖𝑛𝜑 0 0 0
𝑉̃ = 𝑆 𝑇 . 𝑀. 𝑆̇ = [ ][0 𝑚 0] [ 𝜑̇ 𝑠𝑖𝑛𝜑 0] = [ ]
{ 0 0 1 0 0 𝐽 0 0
0 0
𝑣 ̃ −1 . 𝐸̃ . 𝑢
=> 𝑣̇ = [ ] = 𝑀 ( 3.2.4)
𝜔
The error of velocity:
𝑣 𝑣𝑐
𝑣𝑒 = 𝑣 − 𝑣𝑐 = [ ] − [𝜔 ] ( 3.2.5)
𝜔 𝑐
The derivation of velocity error:
̃ −1 . 𝐸̃ . 𝑢 − 𝑣̇𝑐
𝑣̇𝑒 = 𝑣̇ − 𝑣̇𝑐 = 𝑀 ( 3.2.6)
We choose controller law:

𝑢 = 𝐸̃ −1 . 𝑀
̃ . (𝐶. 𝑣̇𝑒 + 𝑣̇𝑐 ) ( 3.2.7)
Where matrix C is a positive definite 2x2 control matrix, we have:
̃ −1 . 𝐸̃ . ( 𝐸̃ −1 . 𝑀
𝑣̇𝑒 = 𝑀 ̃ . (−𝐶. 𝑣𝑒 + 𝑣𝑐 )) − 𝑣𝑐 = −𝐶. 𝑣𝑒 ⇒ 𝑣𝑒 = 𝑒 −𝐶𝑡 ⇒ 𝑙𝑖𝑚 𝑣𝑒 = 0
𝑡→∞
(3.2.8)
200 0
Get C = [ ]
0 100

17
Chapter 4: Simulating control system on MATLAB Simulink

4.1. Kinematic Block

18
4.2. Dynamic Block

19
4.3. Outer Loop Kinematic Controller

20
4.4. Inner Loop Dynamic Controller

21
 Chapter 5: Simulation Result and Conclusion
5.1. Simulation Result
We choose the initial state of WMR: [𝑥(0); 𝑦(0); 𝜑(0)] 𝑇 = [1,2; 1,4; 0] 𝑇
2𝜋
𝑥𝑟𝑒𝑓 = 1.7 𝑠𝑖𝑛 ( 𝑡)
{ 15
2𝜋
𝑦𝑟𝑒𝑓 = 1.7𝑐𝑜𝑠 ( 𝑡)
15
Results of tracking the robot's trajectory:

22
5.2. Conclusion
Even if we deal with a linear but time-varying system, some of the results known
from linear time invariant systems are no longer valid. Here we should mention that
even if all the poles lie in the (fixed) locations of the left half-plane of the complex
plane s, the system might be unstable. In spite of the potential difficulties mentioned
above, linear control laws are often used in practice due to their simplicity, relatively
easy tuning, and acceptable performance and robustness.

23
 Chapter 6: References

[1] J. Hu, P. Bhowmick and A. Lanzon, "Coordinated Control of Networked Mobile Robots with
Applications to Object Transportation," IEEE Transactions on Vehicular Technology, 2021.

[2] J. Hu, P. Bhowmick, I. Jang, F. Arvin and A. Lanzon, "A Decentralized Cluster Formation Containment
Framework for Multirobot Systems," A Decentralized Cluster Formation Containment Framework for
Multirobot Systems.

[3] ". F. C. o. R. S. w. N. D. a. U. D. D. a. Experiments, ""Robust Formation Coordination of Robot Swarms

with Nonlinear Dynamics and Unknown Disturbances: Design and Experiments," "Robust Formation
Coordination of Robot Swarms with Nonlinear Dynamics and Unknown Disturbances: Design and
Experiments.

24