Autonomous Mobile Robots

This repository contains an implementation of the Autonomous Mobile Robots course for ROCV master's program at Innopolis University. The course is instructed by Geesara Prathap. So, this repository contains the course material besides my solutions for the assignments. In addition, I developed PID, LQR controllers for a differential drive robot. Trajectory-tracking error model was developed for applying MPC controller.

The course contents includes:

Motion control (Kinematics, control, and dubins path planning).
Estimation (Kalman filter, extended kalman filter, particle filter).
Localization (Monte carlo, and ekf localization).

Setup

Install at least one simulator: Gazebo
Install the appropriate ROS 2 version as instructed: here.

Clone the repository:

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
git clone https://github.com/Walid-khaled/autonomous_mobile_robots.git

Install dependencies:

cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y

Build and install:
```
cd ~/ros2_ws
colcon build
```

Run

Gazebo-classic

If you had Gazebo installed when compiling Hagen's packages, Gazebo support should be enabled.

Setup environment variables (the order is important):
```
. /usr/share/gazebo/setup.sh
. ~/ros2_ws/install/setup.bash
```
Tip: If the command ros2 pkg list | grep hagen_gazebo comes up empty after setting up the environment, Gazebo support wasn't correctly setup.
Launch Hagen in a city (this will take some time to download models):
```
ros2 launch hagen_gazebo hagen.launch.py world:=hagen_city.world
```
Launch Hagen in an empty world:
```
ros2 launch hagen_gazebo hagen.launch.py world:=hagen_empty.world
```
To avoid these steps, in your terminal, naviagate to the repository directory and make the file "run.sh" executable, then run it to start the simulation directly.
```
cd ~/ros2_ws/src/autonomous_mobile_robots
chmod +x run.sh
./run.sh
```

In a new terminal:

. ~/ros2_ws/install/setup.bash
ros2 run hagen_control desired_controller
desired_controller should be replaced with one of these executables (PID, LQR, MPC)

To reset the simulation:

ros2 service call /reset_simulation std_srvs/srv/Empty

Main results

Control Strategies:

In "hagen_control/hagen_control/hagen_control_strategy.py", specify one of the following controller at the main() and timer_callback() functions:

Control to reference pose
Control to reference pose via an intermediate point
Control to reference pose via an intermediate direction
Reference path control

After the simulation finish, a plot will be genereated to visualize the odometery data with respect to the actual one. For example this is the output for reference path control.

As shown, the odom data diverge by time when reaching the path points without any feedback from the odometery data.

PD Controller

A PD controller was developed to incorporate the odometery data as feedback. It is implemented in 2 stages; reaching the target position and correcting the orientation.

PD_control.mp4

PID Controller

Adding the integral term has affected the removed the steady state error and improved the response. With the parameter tuning, you can obtain better results. Check "hagen_control/hagen_control/diff_drive_PD_control.py".

PID_control.mp4

Linear-Quadratic Controller (LQR)

Two different approaches were developed to solve the LQR problem. Check the formulation and the explanation in the notebooks in "hagen_control/hagen_control/LQR/lqr-01.ipynb". However the approach of the notebook "hagen_control/hagen_control/LQR/lqr-02.ipynb" was adopted as it showed a better performance. This is because

K is recalculated at each timestamp unlike the previous approach.
The Discrete Algebraic Riccati Equation (DARE) is solved for each state (timestamp) using dynamic programming, instead of solving it once as in the previous approach.

So a ros node was developed for LQR implementation, and below are the results. Check "hagen_control/hagen_control/diff_drive_LQR.py".

LQR.mp4

Some notes:

There is an advantage of the LQR controller over the previous PID controller. In LQR, there is no need to control the position and the orientation in 2 separate controllers as we did in PID. In PID, we controlled position and after reaching the position, another controller was applied to correct the orientation which makes a shift in position again. But in LQR, only one controller corrects the position and orientation simultaneously until reaching the desired state (x,y, yaw).
However, I noted that changing the desired state requires to tweak the Q and R matrices again, which is extremely time consuming if it is done manually.

Model Predictive Controller (MPC)

Trajectory tracking error model was developed. Check the formulation and the explanation in the reportin "hagen_control/hagen_control/MPC/MPC_Trajectory_Tracking_Error_Model.pdf" and the MATLAB script "hagen_control/hagen_control/MPC/MPC.m".

Ros node was developed for MPC implementation, and below are the results. Check "hagen_control/hagen_control/diff_drive_MPC.py".

When simulating the robot motion, the following response was obtained:

However, when using the odometry data to update the pose, the following result was obtained:

EKF Localization

Acknowledgement

https://github.com/GPrathap/autonomous_mobile_robots