Tracker Sensor, robot infrared line tracking
Principle of line tracking
Tracker Sensor has five analog outputs, and the outputted data are affected by the distance and the color of the detected object. The detected object with higher infrared reflectance (in white) will make larger output value, and the one with lower infrared reflectance (in black) will make smaller output value. When the sensor is getting close to a black line, the output value will come to smaller and smaller. So it is easy to get the distance from the black line by checking the analog output (The closer distance between the sensor and the black line, the smaller output value you will get). Compare to any other module only with logical (HIGH/LOW) output, we can get more precise result from the analog output of this module. In the following section, we are going to present the algorithm in three parts.
Part 1: Normalization process
Different sensors may output different results for the same color and distance. Furthermore, environment can affect the range of analog output. For example, if we apply 10AD for sampling, we may get the output range from 0 to 1023 theoretically. However, what we get actually will be the Min output value higher than 0 and the Max output value lower than 1023. Normalization process is important and necessary for reducing the affecting factors from different sensors and different environments. Normalization process is a kind of linear transformation by transforming the range of Min~Max to the range of 0~1 with the following formulas.
y = (x - Min) / (Max - Min)
(In which, x is the original output value from sensor, y is the transformed value, and Max and Min are the maximum output value and the minimum output value, respectively.
y = (x - Min) * 1000 / (Max - Min)
After transformed, the output value will be in the range of 0~1000, in which 1000 means the sensor is far away from the black line, and 0 means the sensor is above the black line.
The program will sample the values from the sensors for many times to get the proper value of Min and Max. In order to get the precise Min and Max, the car should be always running in course of sampling.
Part 2: Weighted average
Using normalization process to deal with five sets of output data, we will get five sets of data about the distances between the sensors and the black line. Then, we should use weighted average to transform these data into a value to determine center line of the route with the following formula:
y = (0 * value0 + 1000 * value1 + 2000 * value2 + 3000 * value3 +4000 * value4) / (value0 + value1 + value2 + value3 + value4)
In which, 0, 1000, 2000, 3000, 4000 are the weights for the five detectors, respectively, from left to right. And value0~value4 are the data with normalization process.
Now, we can get the data in the range of 0~4000, which can tell you the position of the black line. For example, 2000 means the black line is in the middle of the module, 0 means the black line is on the leftmost side of the module, and 4000 means the black line is on the rightmost side of the module.
For more precise detection, we have some requirements on the height of the module and the width of the black line. The width of the black line should be equal to or less than the distance of two sensors (16mm). The proper height of the module is that when the black line is in the middle of two sensors, both sensors can detect the black line.
Part 3: PID control
From Part 2, we can get the position of the black line. You should make sure the black line is always under the car, so that the car can run along the black line. So, the output value after weight average process should be kept at 2000. Here, we employ positional PID control to make the car run smoothly. About the PID algorithm, you can easy get a lot of information via Internet. In here, we only have a brief description on it.
PID control can feedback and regulate the error with three factors, proportional (P), Integral (I), derivative (D). The followings are PID algorithm.
proportional = position - 2000; derivative = proportional - last_proportional; integral += proportional; last_proportional = proportional; power_error = proportional * Kp + integral * Ki + derivative * Kd;
Ideally, the weighted average output is 2000, that is, the black line is kept in the middle. The proportional is the result of current position(Position) minus objective position (2000). It is the position error, of which the positive number means the car is on the right of the black line, and the negative number means the car is on the left of the black line.
Integral is the sum of all the errors. When the absolution value is large, the error accumulation is large too, which means the car go far away from the route.
Derivative is the difference of the current proportional and the last proportional, reflecting the response speed of the car. The large derivative value means the fast response speed.
You can adjust the parameters Kp, Ki and kd to have the better performance. Firstly, we adjust Kp; set the Ki and Kd to 0, and adjust the value of Kp to make the car run along the black line. Then, adjust Ki and Kd; set the parameters to a small value or 0.
AlphaBot tracker module example explanation
In this section, we will present the Tracker Sensor program of the AlphaBot smart car. Here, we will take the Arduino program as example. There are two main files in the Tracker Sensor library, TRSensors.cpp and TRSensors.h.
TRSensors.cpp contains the following functions:
TRSensors(); void AnalogRead(unsigned int *sensor_values); void calibrate(); void readCalibrated(unsigned int *sensor_values); int readLine(unsigned int *sensor_values, unsigned char white_line = 0);
in which, TRSensors() is the initial function for pins initialization. And it has been applied for the memory space to store the Max and Min values of the sensors.
AnalogRead() function can read the analog value from 5 channel of sensors. And AlphaBot performs AD conversation via the AD chip of TLC1543, but not the AD pins on the Arduino chip. When you connect the Trackr Sensor to the Pins A0 ~A4 of the Arduino chip, you should modify this function.
calibrate() function is used for calibration, determining the values of Max and Min by sampling the data for many times. In calibration, the car should run along the black line and swing from side to side, to get the precise value of Max and Min
readCalibrated() is for normalization process. From Part 1, we can know that it can transform the output data to the range of 0~1000 by linear transformation, in which 1000 means the sensor is far away from the black line, and 0 means the sensor is above the black line.
readLine() can read the position of the black line. For more detailed information, please refer to Part 2. This function applies weight average to calculate the position of the black line. Then you can get the data in the range of 0~4000, in which 0 means the black line is on the leftmost side of the module, and 4000 means the black line is on the rightmost side of the module.
PID control is implemented in the main function Infrared_Line_Tracking.ino. The following is the relative Demo code.
In this Demo code, we use the function readline() to read the position of the black line, and calculate the proportional and the derivative. Here is PD algorithm not PID algorithm, so there is no integral. If you want a better performance, you can modify the parameters in the PD algorithm.
At last, we use PID algorithm to calculate the iterm power_error, so as to adjust the PWM of the right and the left wheels to make the car run along the black line.
Operation and phenomena
In this section, we only introduce the simple test program of the module. For more information about how to apply AlphaBot to use Tracker Sensor to implement tracking operation and relative Demo, please refer to the corresponding documents of AlphaBot.
① Download the corresponding program into the development board.
② Connect the module to the development board, and connect your PC to the board via a serial cable. Then, start the serial debugging software.
Table 1 and Table 2 show the connection between the module and the development boards.
|Port||Pins of XNUCLEO-F103RB|
Table 1 Connection between the module and STM32
|Port||Pins of Arduino|
Table 2 Connection between the module and Arduino
Table 3 shows the serial port configuration.
Table 3 Serial port configuration
③ The screen will display 5 serial of data for IR1 to IR 5 sensors. All this data will change with the reflective distance. When there is no obstacle in front of the module, the output is probably a few dozen, but when the module is close to the table, its output will go up to about 800~900.