Robot obstacle detection with the ultrasonic ranger

Originally my robot kit was advertised as "avoidance" kit and it does sport a rotating ultrasonic ranger sensor. Previously the DC motors gave me so much trouble that I did not deal with this sensor although the twowdrobot library does provide two low-level functions (TwoWDRobot::set_distance_direction and TwoWDRobot::get_distance) to access the ultrasonic ranger and its servo. So I set out to implement a "patrol" application that discovers an area, similarly to cleaning robots.

My first observation is that the ranger sensor works reliably only if the degree between its main axis and the obstacle is close to 90 degrees. The following diagrams and pictures show 3 situations and the associated readings from the sensor, represented on a 2D diagram. The measurements were made with the twowdrobot_scan sketch that emits the measurement results to the serial port where they can be captured with a terminal emulator software.

The first situation is simple: the sensor points straight to a solid wall. Note however, that the sensor does not detect that the wall is long, even though at 50,60,70,120,130 degrees of sensor direction the wall areas are well within the sensor's range.

The second situation shows when the angle between the robot and the obstacle is small, about 20 degrees. You can see that when the sensor turns sideways (therefore its angle to the obstacle is nearly 90 degrees), the obstacle is detected. However there is no reading at the most important direction, 90 degrees so based on this measurement, the robot will hit the obstacle.

The third situation shows an obstacle that is not completely solid. Here the readings are completely mixed, there is a (quite far) reading at 90 degrees (even though the distance to hit the obstacle is much smaller) and there are much better readings of the obstacle to the right. Based on these measurements, however, the robot would hit the obstacle before the ranger sensor would detect it.

The "patrol" sketch cannot provide a perfect solution to these problems but at least employs some heuristics so that it can go around in a living room without getting frequently stuck.

• The ultrasonic scanning is done in 19 directions (0-180 degrees, 10 degrees steps) and if an obstacle is found, +/- 30 degrees of direction is blocked from the obstacle's direction. This tries to ensure that if an obstacle is detected in some direction, other parts of the obstacle not detected because of less optimal ultrasonic hit degree are still avoided.
• Even though the robot constantly scans the forward direction with the ranger as it drives forward, we cannot trust this measurement because the robot can easily hit something without the sensor noticing it. Therefore the robot advances only as much as the sensor's range (about 0.5 meters) and then does a 180 degree re-scan.
  
• If the TwoWDRobot::timed_straight_drive invocation returns a TIMEOUT condition (this means that it could not reach its destination in 5 seconds) we switch to reverse and back off. This is not perfect, however, because usually the wheels are turning even though the robot is stuck. This means that the distance sensor detects forward movement even if there is none.

The result can be seen in the following video.

You can observe that the quite conservative algorithm does avoid the majority of obstacles even in relatively complex environment. I have to say, however, that there are places in our living room that tricks this algorithm; hole-like places with no solid "walls" around it. Also, obstacles that are lower than the sensor's mounting (doorsteps, for example) cannot be handled by this robot.


How to go forward? The most evident missing sensor is a real distance measurement sensor that measures the actual distance traversed by the robot. The distance measurement sensor that we do have measures the rotation of the wheel and this works great as long as the wheel does not slip. Unfortunately if the robot gets stuck, the wheels normally keep on turning so this sensor is not suitable to detect if the robot is blocked.

I don't know yet where to mount this sensor but the most evident place is the rear wheel which is not driven by a motor.

The PID controller

Key element of our robot's power control is the closed loop comprised of the gyroscope that measures the direction of the chassis ("yaw") and and the motors moving the chassis and eventually causing deviation from the desired direction. The control problem seems simple enough; we would like to keep the direction error 0 (in case of straight drive) or make the error 0 (in case of turning). The reality is that there will be error, however. In case of turning the large error is already present at the beginning of the movement (because the chassis is oriented in a different direction than the desired one), in case of straight drive the error will appear inevitably as the imperfect power estimation, small obstacles, etc. will cause the robot to deviate from the "straight" direction. This post explains, how the robot software handles this error.

This problem is more complicated than it seems. For starter, the inertia of the robot chassis is always present. Just because we switch off the motors, the chassis will keep moving. In case of turning, one can typically calculate with 20 degrees of rotation after the motors have been switched off. The exact correction tag depends on a lot of factors that are impossible to calculate in closed form as our robot kits exhibit quite a variety in terms of mechanical parameters. So the approach is to adapt the motor power according to the errors that we measure and report success if the error is small enough.

One time-tested algorithm able to perform such a control is the PID controller. The algorithm is more than 100 years old and was originally developed for torpedo depth control. The assumption is that we have an error that is calculated from desired and measured values of a parameter and we would like to calculate an actuator signal that influences the controlled system. In our robot, the error value is calculated from the desired and real direction angle and the actuator signal is the relative power of the motors. These relative values will be turned into PWM values by two calibration tables as described in the previous post.

The PID controller's output is used differently in the straight drive and turning movements. In case of the straight drive the user specifies the power applied to the robot's wheels. The wheel on the same side as the deviation gets this power, the wheel opposite to the deviation gets user-defined power minus PID controller output. In case of the turning movement, the wheels receive the power determined by the PID controller's output but in opposite direction.

Check out TwoWDRobot::motor_pid_controller for the implementation of our PID controller. The proportional-integral-derivative parameters have been found out by some experimentation and I do not claim that the tuning cannot be more perfect. Consider, however, the diagrams below that show the operation of the controller in case of 4x90 degree turns, performed by the same robot, at the same location, in immediate succession. The experiment was done on a parquet which is somewhat slippery and you can observe the same oscillation movement that can be seen in this video when the robot turns.

So here are the four diagrams.

There are obviously large differences that can be explained by the control system's timing relative to the state of the mechanical system.  The controller's timing is determined by the gyro's 100 Hz sampling rate. This sounds fast enough but if you consider that the entire turn is over in about 1.5 sec (that means 150 gyro samples), you can realize too that no two movements are the same from the digital system's point of view. The PID controller's tuning must also consider different soil types and the quite large variety of the DC motors that come with the robot kits. I found this "aggressively tuned" controller appropriate for my purposes.

Calibrating DC motors

2-wheeled robots need constant adjusting of motor power. In our robot, the input for power management is the gyroscope, if the robot deviates from the prescribed direction, the gyroscope measures the deviation and the motor control logic intervenes. If the initial power estimation for the motors is not exact enough, this post-effect control results in a path that is very far from straight line. It is important therefore to have a good initial estimation of motor power.

The following two images show the relative motor power of the two robots we built as a function of the PWM value applied by the microcontroller. The y axis is measured as the total distance run by the specified motor in a specified time interval when applied the PWM value on the x axis. Y values are scaled, the longest distance is 100% and all the other distances are scaled accordingly. Blue line is motor#1, green line is motor#2.

 

Power ratio of the two motors in case of my robot

 Power ratio of the two motors in case of my son's robot

As the diagrams show, DC motors of this low-cost robot kit are very different. Therefore a calibration step was introduced. Once you assembled the robot and made sure that the wheels are indeed spinning in the right direction (check out this sketch for basic motor test), you should upload the calibration sketch and execute it. This sketch measures the power curves shown above by applying increasing power to one of the motor and measuring the distance run with the specified power in 3 seconds. As only one motor is tested at a time, the robot will rotate so give the robot enough space and a stable, even surface.

The calibration sketch saves the measured values that the robot library uses later. Check out TwoWDRobot::init_motors() to see, how the calibration data stored in EEPROM is read and TwoWDRobot::get_pwm_from_power_percentage to see, how the PWM value is calculated from the power percentage value.

Additions to the assembly guide

There is a detailed assembly guide on the project page (also in Hungarian). There were some questions, however, that I found better to address in this blog area.

First the assembly style. The assembly guide just states that electronic boards, etc. should just be attached to the chassis. How you do it is a style question. For example the robot that my son assembled sports a low profile.

My version is less elegant and also less stable mechanically as the boards are all stacked onto the back of the chassis.

I have a good reason for clearing up the front part of the robot - I am already preparing for the next steps when additional embedded computer (or computers) will be added to provide higher-level functions like image processing. The front part is ready to accept those cards.

Next, about the battery. Arduino Uno's recommended input voltage range is 7-12V. So you need at least a 7V battery pack. My solution is a 2-pack Li-Ion battery pack as shown in the image below.

This should not be an issue, however. Use whatever battery you have at hand provided that its voltage is in the 7-12V range. Be advised, however, that 4x1.2V NiMh batteries that fit into the battery holder that usually come with these robot kits will not provide enough voltage. If you stick to 1.2V AA batteries (rechargeable batteries frequently found in supermarkets), you need at least 6 of those. If you follow my advice and go for a type 14500 Li-Ion battery, don't forget to obtain also a charger. Even though type 14500 comes in AA form factor, ordinary AA chargers designed for the 1.2V NiMh batteries will not charge them.

Also, be careful that you can damage the Li-Ion battery by discharging them below the safe voltage. This is not a serious issue for our robot because the motors will stop functioning long before the batteries are discharged below the safe voltage. But if you leave the robot stuck somewhere with the batteries turned on, you can get to this situation.