Goliath Fall 2016
Control Algorithm Using Triangulation
By: Sou Thao (Electronics and Control Engineer)
Approved by Kristen Oduca (Project Manager)
Table of Contents
Introduction
Requirement: The Goliath shall follow the biped at a distance of 20 inches with 15% margin of error.
In order to follow BiPed autonomously within a range of 20 inches, a new control algorithm needs to be created. The control algorithm translates the sensor’s values into Pulse Width Modulated values to control the motors until the Goliath has reached a set point. In turn, the control algorithm should incorporate triangulation in order to control its motors. Triangulation consists of determining a location using the geometric formulas of a triangle including the laws of sine and cosine. By finding the sides and angles of the triangle formed by the sensors and the object, we will be able to use our control algorithm to keep the middle distance and middle angle at a designated set point near 20 inches and 90° respectively.
Determining an Object’s Distance Using Triangulation
From Figure 1, we can see that when both sensors detect an object, we get a triangle.
2c represents the distance both sensors are separated, a represents the distance the object is detected by the left sensor, and b represents the distance the object is detected by the right sensor. The distance d represents the distance from the center of the two sensors to the object and the angles A, B, and C correspond to their sides respectively. Angle θ is the angle where line d intersects the line 2c. In our case, we want to find the distance d and the angle θ so we can develop a control algorithm to keep those values close to our set point of 20 inches and 90°. To find both of them, we have to use the laws of sine and cosine. By looking at the big triangle, according to Wolfram MathWorld shown in Figure 2 [1].
Figure 2 – Law of Cosine
From the equations above, we were able to determine the distance d and the angle θ based on the readings of the sensors and the distance between them. After finding these values, we were able to create two PI controllers: the vertical speed will be controlled by the error in d, and the horizontal speed will be controlled by the error in θ. The vertical speed will tell us how fast to move forward while the horizontal speed will give us the difference in speed between the two motors. Because we should follow BiPed within a distance of 20 inches or 50 centimeters with a 15% margin of error, we chose the tracking distance to be 18 inches or 45 centimeters which creates a 10% margin. The reason we chose to follow BiPed at 18 inches is because from our blog post on the updated sensor’s field of view test (link to Updated MaxSonar Field of View Test), we determined that we were able to get better horizontal resolution the closer we were to BiPed. Thus, our set point for the controller involving distance d is 45 centimeters. Next, our set point for the angle θ is 90° or 1.57 radians. The reason why we need the angle to be 90° is because we want the object to be perpendicular to the midpoint between both sensors. This will make sure both the sensors are aligned and will straighten out the front of Goliath to face BiPed head on.
Fixed Errors Caused by Triangulation
Initially, when we wrote the first algorithm, we noticed many errors where the values of theta and d returns nan or not a number. The reason we are getting this error is because sometimes the sensors’ values do not create a perfect triangle. This is caused by the object tracked because it is not perfectly a point in the sensor’s field of view. Instead, the object has legs so when the sensors see the object, it will detect the closest part of the object. For example, the left sensor will detect the left leg of BiPed while the right Sensor will detect the right leg of BiPed. This will give us an inaccurate triangle and we will not be able to determine the distance nor the angle.
In order to fix this error, we know that a triangle can only be formed by having the sum of the 2 greater sides being greater than the longest side [2]. Therefore, we have to find a solution to solve this problem shown in Figure 3.
Figure 3 – Calculating Fixed Error
Using RGB LEDs to Help Troubleshoot
In order to fine tune the kp and ki constants for both the vertical and horizontal controllers, the five RGB LEDs were used to troubleshoot errors and overshoot caused by the controllers. These LEDs performed perfectly in letting us know what Goliath is seeing as it is moved using the controlled algorithm. Thus, we were able to adjust different parameters to make a perfect control system to track an object accurately.
Writing the Triangulation Control Algorithm
//store the sensors values into variables a and b
double a = leftSensorAverage;
double b = rightSensorAverage;
//c represents the half the distance between both sensors
double c = 9.375;
//Fix the Distances if There is an Error in Creating a Triangle
if (a>b) {
b = adjust(a,b);}
else {
a = adjust(b,a);}
//determining the direction the object is at
double horizontalDirection = a-b;
//using triangulation to find distance d and theta angle
//please refer to Control Algorithm Using Triangulation
//on Goliath Blog Posts for More Information
double angleD = acos((sq(a)-sq(b)+(sq(2*c)))/(4*a*c));
double d = sqrt(sq(a)+sq(c)-(2*a*c*cos(angleD)));
double theta = asin(a/d*sin(angleD));
//Initialize the set point of the vertical distance to be 45cm
int dT = 45;
//Find the Vertical Error
int verticalError = dT – d;
//Set an initial speed where motors are barely going forward
int minVerticalSpeed = 140;
/*//////////////////////////////////////
*PI Contrller For Verical Control/////
*/////////////////////////////////////
//Setting Up the P Controller
int kpVertical = 0.7;
float PVertical = abs(kpVertical*verticalError);
//Setting Up the I Controller
int kiVertical = 1;
//If the vertcal distance d is closer to the set point make
//integral term equal to 0 otherwise increase the integral term
if (abs(verticalError)> 5){
verticalIntegral = verticalIntegral+abs(verticalError);
}
else {
verticalIntegral = 0;
}
int IVertical = abs(verticalIntegral*kiVertical);
//Set the Vertical Speed to the sum of the output from the PI Controller
float verticalSpeed = minVerticalSpeed+PVertical+IVertical;
//Make the horizontal set point equal to 1.5708
double horizontalError = 1.5708-theta;
//Set an initial speed to differentiate between the motors speed
int minHorizontalSpeed = 20;
/*//////////////////////////////////////
*PI Contrller For Horizontal Control///
*/////////////////////////////////////
//Setting Up the P Controller
int kpHorizontal = 50;
float PHorizontal = abs(kpHorizontal*horizontalError);
//Setting Up the I Controller
int kiHorizontal = 10;
//If the horizontal angle is closer to the set point make
//integral term equal to 0 otherwise increase the integral term
if (abs(horizontalError)> .4){
horizontalIntegral = horizontalIntegral+abs(horizontalError);
}
else {
horizontalIntegral = 0;
}
int IHorizontal = abs(horizontalIntegral*kiHorizontal);
//Set the Horizontal Speed to the sum of the output from the PI Controller
float horizontalSpeed = minHorizontalSpeed+PHorizontal+IHorizontal;
//If the Object is further than the set point and it’s located
//to the right make the left motor run faster than the right motor
//to make a left turn going forward
if ((verticalError<0)&&(horizontalDirection>0)){
speedLeft = verticalSpeed + horizontalSpeed;
speedRight = verticalSpeed – horizontalSpeed;
}
//If the object is further than the set point and it’s located
//to the left make the right motor run faster than the left motor
//to make a right turn going forward
else if ((verticalError<0)&&(horizontalDirection<0)) {
speedLeft = verticalSpeed – horizontalSpeed;
speedRight = verticalSpeed + horizontalSpeed;
}
//If the object is further than the set point and it’s located
//in the middle make both motors run faster going forward
else if ((verticalError<0)&&(horizontalDirection==0)) {
speedLeft = verticalSpeed+horizontalSpeed;
speedRight = verticalSpeed+horizontalSpeed;
}
//If the object is closer than the set point and it’s located
//to the right make the left motor slower than the right motor
//to make a right turn going backwards
else if ((verticalError>0)&&(horizontalDirection>0)) {
speedLeft = verticalSpeed – horizontalSpeed;
speedRight = verticalSpeed + horizontalSpeed;
}
//if the object is closer than the set point and it’s located
//to the left make the right motor slower than the left motor
//to make a left turn going backwards
else if ((verticalError>0)&&(horizontalDirection<0)) {
speedLeft = verticalSpeed + horizontalSpeed;
speedRight = verticalSpeed – horizontalSpeed;
}
//if the object is closer than the set point and it’s located
//in the middle make both sensors run faster going backwards
else if ((verticalError>0)&&(horizontalDirection==0)) {
speedLeft = verticalSpeed + horizontalSpeed;
speedRight = verticalSpeed + horizontalSpeed;
}
//if the object reaches the set point, don’t move the motors
else if (verticalError==0) {
speedLeft = 0;
speedRight = 0;
}
//control the speed of the PWM to be within 0 to 255
if (speedLeft >255) {
speedLeft = 255;
}
else if (speedLeft < 0) {
speedLeft = 0;
}
if (speedRight >255) {
speedRight = 255;
}
else if (speedRight < 0) {
speedRight = 0;
}
//If the object is on the right and it is below
//the range of the set point adjust the brightness
//of the two right red LEDs
if ((a>b)&&(d<42)) {
brightness = map(d,20,42,10,500);
//LED1 Furthest Right LED
pwmSensors.setPWM(12,0,brightness);
pwmSensors.setPWM(13,0,0);
pwmSensors.setPWM(11,0,0);
//LED2
pwmSensors.setPWM(9,0,brightness);
pwmSensors.setPWM(10,0,0);
pwmSensors.setPWM(8,0,0);
//LED3
pwmSensors.setPWM(6,0,0);
pwmSensors.setPWM(7,0,0);
pwmSensors.setPWM(5,0,0);
//LED4
pwmSensors.setPWM(3,0,0);
pwmSensors.setPWM(4,0,0);
pwmSensors.setPWM(2,0,0);
//LED5 Furthest Left LED
pwmSensors.setPWM(0,0,0);
pwmSensors.setPWM(1,0,0);
pwmSensors.setPWM(14,0,0);
}
//If the object is on the right and it is within
//the range of the set point adjust the brightness
//of the two right green LEDs
else if ((a>b)&&((d>=42) && (d<=48))) {
brightness = map(d,42,48,10,500);
//LED1 Furthest Right LED
pwmSensors.setPWM(12,0,0);
pwmSensors.setPWM(13,0,brightness);
pwmSensors.setPWM(11,0,0);
//LED2
pwmSensors.setPWM(9,0,0);
pwmSensors.setPWM(10,0,brightness);
pwmSensors.setPWM(8,0,0);
//LED3
pwmSensors.setPWM(6,0,0);
pwmSensors.setPWM(7,0,0);
pwmSensors.setPWM(5,0,0);
//LED4
pwmSensors.setPWM(3,0,0);
pwmSensors.setPWM(4,0,0);
pwmSensors.setPWM(2,0,0);
//LED5 Furthest Left LED
pwmSensors.setPWM(0,0,0);
pwmSensors.setPWM(1,0,0);
pwmSensors.setPWM(14,0,0);
}
//If the object is on the right and it is above the
//range of the set point adjust the brightness of
//the two right blue LEDs
else if ((a>b)&&(d>48)) {
brightness = map(d,48,255,10,4095);
//LED1 Furthest Right LED
pwmSensors.setPWM(12,0,0);
pwmSensors.setPWM(13,0,0);
pwmSensors.setPWM(11,0,brightness);
//LED2
pwmSensors.setPWM(9,0,0);
pwmSensors.setPWM(10,0,0);
pwmSensors.setPWM(8,0,brightness);
//LED3
pwmSensors.setPWM(6,0,0);
pwmSensors.setPWM(7,0,0);
pwmSensors.setPWM(5,0,0);
//LED4
pwmSensors.setPWM(3,0,0);
pwmSensors.setPWM(4,0,0);
pwmSensors.setPWM(2,0,0);
//LED5 Furthest Left LED
pwmSensors.setPWM(0,0,0);
pwmSensors.setPWM(1,0,0);
pwmSensors.setPWM(14,0,0);
}
//If the object is on the left and it is below
//the range of the set point adjust the brightness
//of the two left red LEDs
else if ((a
brightness = map(d,20,42,10,500);
//LED1 Furthest Right LED
pwmSensors.setPWM(12,0,0);
pwmSensors.setPWM(13,0,0);
pwmSensors.setPWM(11,0,0);
//LED2
pwmSensors.setPWM(9,0,0);
pwmSensors.setPWM(10,0,0);
pwmSensors.setPWM(8,0,0);
//LED3
pwmSensors.setPWM(6,0,0);
pwmSensors.setPWM(7,0,0);
pwmSensors.setPWM(5,0,0);
//LED4
pwmSensors.setPWM(3,0,brightness);
pwmSensors.setPWM(4,0,0);
pwmSensors.setPWM(2,0,0);
//LED5 Furthest Left LED
pwmSensors.setPWM(0,0,brightness);
pwmSensors.setPWM(1,0,0);
pwmSensors.setPWM(14,0,0);
}
//If the object is on the left and it is within
//the range of the set point adjust the brightness
//of the two left green LEDs
else if ((a
brightness = map(d,42,48,10,500);
//LED1 Furthest Right LED
pwmSensors.setPWM(12,0,0);
pwmSensors.setPWM(13,0,0);
pwmSensors.setPWM(11,0,0);
//LED2
pwmSensors.setPWM(9,0,0);
pwmSensors.setPWM(10,0,0);
pwmSensors.setPWM(8,0,0);
//LED3
pwmSensors.setPWM(6,0,0);
pwmSensors.setPWM(7,0,0);
pwmSensors.setPWM(5,0,0);
//LED4
pwmSensors.setPWM(3,0,0);
pwmSensors.setPWM(4,0,brightness);
pwmSensors.setPWM(2,0,0);
//LED5 Furthest Left LED
pwmSensors.setPWM(0,0,0);
pwmSensors.setPWM(1,0,brightness);
pwmSensors.setPWM(14,0,0);
}
//If the object is on the left and it is above the
//range of the set point adjust the brightness of
//the two left blue LEDs
else if ((a
brightness = map(d,48,255,10,4095);
//LED1 Furthest Right LED
pwmSensors.setPWM(12,0,0);
pwmSensors.setPWM(13,0,0);
pwmSensors.setPWM(11,0,0);
//LED2
pwmSensors.setPWM(9,0,0);
pwmSensors.setPWM(10,0,0);
pwmSensors.setPWM(8,0,0);
//LED3
pwmSensors.setPWM(6,0,0);
pwmSensors.setPWM(7,0,0);
pwmSensors.setPWM(5,0,0);
//LED4
pwmSensors.setPWM(3,0,0);
pwmSensors.setPWM(4,0,0);
pwmSensors.setPWM(2,0,brightness);
//LED5 Furthest Left LED
pwmSensors.setPWM(0,0,0);
pwmSensors.setPWM(1,0,0);
pwmSensors.setPWM(14,0,brightness);
}
//If the object is in the middle and it is below the
//range of the set point adjust the brightness of
//the three middle red LEDs
else if ((a==b)&&(d<42)) {
brightness = map(d,20,42,10,500);
//LED1 Furthest Right LED
pwmSensors.setPWM(12,0,0);
pwmSensors.setPWM(13,0,0);
pwmSensors.setPWM(11,0,0);
//LED2
pwmSensors.setPWM(9,0,brightness);
pwmSensors.setPWM(10,0,0);
pwmSensors.setPWM(8,0,0);
//LED3
pwmSensors.setPWM(6,0,brightness);
pwmSensors.setPWM(7,0,0);
pwmSensors.setPWM(5,0,0);
//LED4
pwmSensors.setPWM(3,0,brightness);
pwmSensors.setPWM(4,0,0);
pwmSensors.setPWM(2,0,0);
//LED5 Furthest Left LED
pwmSensors.setPWM(0,0,0);
pwmSensors.setPWM(1,0,0);
pwmSensors.setPWM(14,0,0);
}
//If the object is in the middle and it is within the
//range of the set point adjust the brightness of
//the three middle green LEDs
else if ((a==b)&&((d>=42) && (d<=48))){
brightness = map(d,42,48,10,500);
//LED1 Furthest Right LED
pwmSensors.setPWM(12,0,0);
pwmSensors.setPWM(13,0,0);
pwmSensors.setPWM(11,0,0);
//LED2
pwmSensors.setPWM(9,0,0);
pwmSensors.setPWM(10,0,brightness);
pwmSensors.setPWM(8,0,0);
//LED3
pwmSensors.setPWM(6,0,0);
pwmSensors.setPWM(7,0,brightness);
pwmSensors.setPWM(5,0,0);
//LED4
pwmSensors.setPWM(3,0,0);
pwmSensors.setPWM(4,0,brightness);
pwmSensors.setPWM(2,0,0);
//LED5 Furthest Left LED
pwmSensors.setPWM(0,0,0);
pwmSensors.setPWM(1,0,0);
pwmSensors.setPWM(14,0,0);
}
//If the object is in the middle and it is above the
//range of the set point adjust the brightness of
//the three middle blue LEDs
else if ((a==b)&&(d>48)){
brightness = map(d,48,255,10,4095);
//LED1 Furthest Right LED
pwmSensors.setPWM(12,0,0);
pwmSensors.setPWM(13,0,0);
pwmSensors.setPWM(11,0,0);
//LED2
pwmSensors.setPWM(9,0,0);
pwmSensors.setPWM(10,0,0);
pwmSensors.setPWM(8,0,brightness);
//LED3
pwmSensors.setPWM(6,0,0);
pwmSensors.setPWM(7,0,0);
pwmSensors.setPWM(5,0,brightness);
//LED4
pwmSensors.setPWM(3,0,0);
pwmSensors.setPWM(4,0,0);
pwmSensors.setPWM(2,0,brightness);
//LED5 Furthest Left LED
pwmSensors.setPWM(0,0,0);
pwmSensors.setPWM(1,0,0);
pwmSensors.setPWM(14,0,0);
}
//If the difference between both LEDs is
//greater than 15, we are lost so turn on
//all red LEDs
if (abs(a-b)>15){
//LED1 Furthest Right LED
pwmSensors.setPWM(12,0,500);
pwmSensors.setPWM(13,0,0);
pwmSensors.setPWM(11,0,0);
//LED2
pwmSensors.setPWM(9,0,500);
pwmSensors.setPWM(10,0,0);
pwmSensors.setPWM(8,0,0);
//LED3
pwmSensors.setPWM(6,0,500);
pwmSensors.setPWM(7,0,0);
pwmSensors.setPWM(5,0,0);
//LED4
pwmSensors.setPWM(3,0,500);
pwmSensors.setPWM(4,0,0);
pwmSensors.setPWM(2,0,0);
//LED5 Furthest Left LED
pwmSensors.setPWM(0,0,500);
pwmSensors.setPWM(1,0,0);
pwmSensors.setPWM(14,0,0);
}
//If the object is too close
//from set point go backwards
if (verticalError>0){
motorA.go(2,speedLeft);
motorB.go(2,speedRight);
}
//If the object is too far
//from set point go forwards
else if (verticalError<0) {
motorA.go(1,speedLeft);
motorB.go(1,speedRight);
}
//if the object is within range
//of set point stop moving
else if (verticalError==0) {
motorA.go(1,0);
motorB.go(1,0);
}
//fix the shorter side if a triangle
//is not able to be formed
float adjust(float Sl,float Ss)
{
float c = 18.75;
if (Sl >= (Ss+c)){
float extra = Sl-(Ss+c)+1;
Ss = Ss+extra;}
return Ss;
}
Figure 4 – Tracking Biped from the Middle
Figure 5 – Tracking Biped from the Right
Figure 6 – Tracking Biped from the Left
Conclusion
From our tests in developing the control algorithm, we were able to fine tune the two PI controllers by choosing these settings:
Vertical PI Controller: Minimum Speed = 140, kp = 0.7, ki = 1
Horizontal PI Controller: Minimum Speed = 20, kp = 50, ki = 10
As a result, Goliath was able to track an object at a distance of 18 inches accurately and it stayed within the range of 20 inches with a 10% margin of error. There were no jitters caused in the constant changes of the motor’s values and the motors ran smoothly as it tracked the object. Also, Goliath was able to move backwards and set itself within the designated set point. This algorithm proved that using triangulation was an effective way to implement an autonomous system with the MaxSonar sensors. Furthermore, this algorithm will be able to assist future developers and robot enthusiasts in their projects involving control systems for autonomous vehicles and robotic systems.