25. Sumo Project / Adventures in Control Theory and Optimization

Post date: Jul 01, 2016 1:5:8 AM

Prototype 1. This is the first prototype. It will primarily focus on tracking. Looking pretty will be a later prototype.

So I have recently been applying some control theory in the project, and I am having fun doing it too. It's funny considering, during my years taking didactic coursework, I have never taken a single class in controls. This is probably pretty odd for someone who majors in Electrical Engineering. So, I decided the Sumo Project is the perfect opportunity to learn the basics while optimizing the robot's tracking. After some intense research ( i.e. googling and mostly reading the control theory and related pages on Wikipedia ), I determined how to best model my particular problem.

I got this figure from Wikipedia's page on Control Theory.

I instantly picked the Proportional Integral Derivative (PID) --- rather, I picked the Proportional Summation Difference since the system operates in Discrete-Time --- as the controller due to how commonplace the controller is. Plus, the PID controller on its own is really simple to apply. The challenge then is how to determine a mathematical model that best represents the System ( which is also referred to as the Plant, apparently ). I should point out I simplify the model by combining both the System and Sensor as System.

PID controller in Discrete-Time. e[n] is the Error. r[n] and y[n] are the Reference and the Measured Output, respectively. u[n] is the System Input. Finally, Kp, Ki, and Kd are the constant parameters that alter the performance of the controller.

Let me first define the objective: The project's first prototype needs to lock onto our opponent, without accidentally overshooting. Another way of describing the same problem is that our robot needs to correct its orientation to the opponent so that our robot is facing directly to the opponent. The Measured Output therefore represents the angle our robot is currently pointing, assuming 0 radians is the direction of our opponent and also the Reference. Simply negating the Measured Output results in the Error. The System Input should be a control variable that corrects the Measured Output. So, it follows the System Input becomes the angle needed to correct our robot's current orientation to the opposing robot.

However, the direct inputs consist of distance values acquired from IR Sensors and the direct outputs are speed values sent to each of the two motors. Well, the System Input is proportionally related to angular velocity, basically how much the direction of the robot changes in a single unit of time. Angular velocity is of course proportional to tangential speed, which --- from my current understanding --- should also be proportional to the speeds I can adjust the motors to. Haven't given too much thought on the relationship between the angular velocity and the speeds of the motors. That will be one of the later steps.

y[n] is the robot's direction to the opponent and the average angle among all the sensors ( and the Measured Output). Theta0 is the direction of the first sensor to the opponent. ThetaSensor is the angle between each adjacent sensor. I is the total number of sensors.

So, how do we acquire the robot's direction to the opposing robot, with only knowing the distance between each IR Sensor and the opponent? A part of the answer is the set of equations above. The Measured Output is the average angle of all the angles associated with each IR Sensor, once again assuming 0 radians is defined by where the opponent is, relative to our robot. Unfortunately, since the PIC32 needs to rely on trigonometric functions implemented in software, it's important to minimize the number of trigonometric operations. Fortunately, in this case, the average angle operation can be completely avoided since angle between each sensor and the number of sensors are known. The problem now becomes how do we compute the angle of the first sensor from the opponent?

The system of equations --- each sensor ( i ) will have its own equation --- is derived from the figure above. The dotted circle represents how the robot can turn and the possible locations of the sensors. The origin represents the center of the robot. The blue circle is the opponent to which our robot is trying to lock on. Since the objective is defined such that our robot's orientation is always relative to the opponent, the blue circle can only move along the x-axis between the dotted circle's radius and infinity ( Ox ). The orange circles represent the sensors. Know that the figure only shows two sensors since, at the minimum, two sensors are needed to provide enough information. However, the model is generalized such that any number of sensors, in theory, can be applied. Finally, the length of each of the blue lines is always equal to the radius of the dotted circle ( R ), whereas the length of each orange line is the distance between the respective sensor and the opponent ( Di ) .

The answer is the system of equations shown above with the figure! I'm not going to go through the whole derivation. Just know it's derived from the distance formula --- specifically, the distance between each sensor and the opponent --- and the fact that the angle between each sensor is known. So long as there are at least two sensors, the angle of the first sensor can be computed by solving the system of equations --- a non-linear set of equations, that is. Now how do we solve this sort of problem, especially given that a precise solution may not exist due to noise or other disturbances found in the real application?

Gradient Descent. The bold variables are either vectors or matrices. j refers to each iteration of the algorithm. i not only refers to each sensor, but also each row in G. J is the Jacobian. Finally, Gamma is a constant that controls the degree by which V updates. Please note that [n] is implied for Di, Ox, and Theta0. However, the reason for including [n-1] for V0 is to point out the initial guess for the gradient descent algorithm is always the previous results.

An excellent solution is to apply an optimization algorithm since they will converge to the closest possible solution, therefore ensuring there is always a solution to work with. The chosen optimization algorithm in this case is gradient descent, simply because --- well --- it's simple, and judging from my MATLAB simulations, it does the job. Instead of relying solely on Mean Square Error as the stop condition, I included a limitation on the maximum allowable iterations so as to also limit the maximum computational time.

Finally, at last, all the pieces of the tracking algorithm have been determined! I'm only going to briefly go over how I constructed the simulation in MATLAB, that way we can quickly get to the simulated results! In a later post I will discuss in more detail how the distance values --- the only information the robot has access to --- is artificially generated. For now, just know that the distance values are computed from made up opponent distances and the true orientation of the robot.

Since the gradient descent algorithm is limited to only a few iterations and there is always going to be real-world disturbances, the true orientation will more than likely differ from what the robot computes with the optimization algorithm. True orientation in most of my simulations is computed as the sum between the previous true orientation and the System Input, the output of the Controller. Including another term in the sum is a way of modeling an unknown, such as the angular velocity of the opponent around the robot.

Sample Simulation 1.

Sample Simulation 2.

I can generate more results, but I think the two Sample Simulations demonstrate enough for this post. After implementing the algorithm and making sure it can track an object slowly, I'll work on optimizing the parameters of the algorithm --- specifically the parameters of the PID controller and the maximum number of iterations for which the gradient descent algorithm can run.