Hello everyone!

Last semester we had a course on Control Systems, where we learned about controlling systems and how to design their controllers. As a huge fan of Boston dynamics Spot and their various robots, I’ve always been curious about how they work.

The very first time I was introduced to robotics was back in 5th grade’s Summer Camp, where we had to build robots like Line following and Path navigating.

Now coming back to the current project, This project is currently my Final year project, and I’m still working on it. But that doesn’t stop me from explaining the simulation built-in Godot.

The Project

If you wanna skip to the final part, you can go to the Web Version

The main goal of this project is to build a simulation of a rocket controller, which can stabilize the rocket in a controlled manner. For this, the most naive approach is to use a PID controller to stabilize the rocket.

A Proportional, Integral and Derivative controller (PID) is a traditional controller that can be used to stabilize a system. As the name suggests, the PID controller is a combination of a Proportional, Integral and Derivative controller, given by the following equation:

$$F_{out} = k_p * \delta + k_d * \frac{d}{dt} \delta + \int_{0}^{t} k_i * \delta dt$$

$$ \delta \textrm{ is the error between the desired value and the actual value. }$$

That’s a heck of a lot of math, but it’s really simple to understand.

Now if you’re any intelligent living specimen, It’ll be obvious to move the needle to the right by +23 degrees, and viola the error is 0.

This is the proportional controller, which is the simplest of the three. It simply tries to move the needle to the right by +23 degrees, which is what we did.

$$ F_{P} = k_p * \delta $$

Now we can see the error is oscillating from -23 deg to +23 deg, Which is where the derivative comes in, which crushes down the oscillating errors.

$$F_{D} = K_d * \frac{d}{dt} \delta$$

Here there’s a DC error that isn’t oscillatory nor linear, For this DC error, we need to use an Integral controller.

$$ F_{I} = K_i * \int_{0}^{t} \delta dt$$

Now, why did I tell you all this? Well, this is the entirety of the PID controller.

The Rocket is a simple rigid body, where torque is applied to the rocket to rotate it and Force is applied as thrust to propel it.

The Thrust force can be rotated to change the direction of the force being applied, this is Thrust Vectoring.

For the simplicity of control, Rocket’s z rotation is locked to 0 deg.

Now for any Control System, we have both Control parameters and Observer parameters. For this rocket, I’ve chosen the Control parameters to be the X, Y torque which is controlled by side thrusters, and the main thrust along with thrust vectoring controlled by their respective parameters, For the Observer parameters we have the Z position (Altitude) along with X, Y Rotation for keeping the rocket upright.

Now for just settings all the PID controllers and observer parameters and tuning it just right.

The Simulation

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12

   #rotational side thrusters
   PID_X = preload("res://PID.gd").new(1, 0.5, 0.6)
   PID_Z = preload("res://PID.gd").new(1, 0.5, 0.6)
   #thrust vectoring
   PID_tv_x = preload("res://PID.gd").new(0.4, 0.2, 0.15)
   PID_tv_z = preload("res://PID.gd").new(0.4, 0.2, 0.15)
   #height calib
   PID_Ypos = preload("res://PID.gd").new(0.5, 0.35, 0.75)
   PID_tv_x._set_range(-0.3,0.3)
   PID_tv_z._set_range(-0.3,0.3)
   PID_Ypos._set_range(0,1)
   PID_Ypos.minimum_clamp = 0; # can't get negative thrust right

The PID values chosen as tuned experimentally, although there are much better ways of properly tuning it and getting the best results the first set of values chosen “educatively..lol” just worked out of the box.

I know tuning is done by computing the transfer function of the PID system I’ve got going but I’m planning on switching to MATLAB for actual simulation so this project just deals with the basic control scheme and manual tuning.

As you can see there’s a lot of oscillation and drift on the X Y plane, But it does seem to stabilize itself upright so that’s progress.

After a bit of adjusting the Derivative part and adding an observer for the X Y position here’s the result.

Auto landing

By setting the target rotating and position to a particular point, The rocket tries to reach the specified location. By generating a path towards a landing zone and interpolating the position, We can land the rocket!.

1
2
3
4
5
6
7
8
9

...
   if(autopilot):
       err_rot_x = target_rot.x - rx;
       err_rot_z = target_rot.z - rz; #vec2 y = z blender coord space
  
       err_pos_x = target_pos.x - px;
       err_pos_z = target_pos.z - pz;
       err_pos_y = target_pos.y - height; 
...

Overall this has been a very fun project, I’ve learned a lot about building PID controllers and using it, although in a Simulation it’s still a very fun thing to play around with. For my major project, I’m planning on using this as a base to build a practical implementation. Till that time keep exploring :D.

Web version

A web playable version of this project is available Here