Robotics: How to Use an LED to Determine Distances

John Keogh | May 1, 2013

I looked at a few options for measuring distance for a small robot I'm working on, but it occurred to me that I should be able to have the robot simply shine an LED on something, and, based on the amount of light returned and the scatter of the light, be able to determine how far an object is from the robot. This post describes how the experiment worked and how to do it yourself. The language used is Objective-C, but the algorithm should be straight-forward to port if you are using another language.

This blog post has an accompanying video which briefly shows the robot that I mentioned above in action, and also demonstrates using the technology this blog post covers:

The accuracy of the technique is reasonable, with the inaccuracy mostly related to the automatic adjustment of the iDevice camera to changed ambient light levels when the LED is switched on.

Approach

These are the things that need to be done:

Make sure that the LED headlights are off
Pause for iDevice light level adjustment
Save image - this is the headlight off image
Turn LED headlights on
Do not pause for iDevice light level adjustment, the next image is saved quickly
Save image - this is the headlight on image
Compare pixel luminosity of headlight off and headlight on images.
take the inverse square of the ratio of changed pixels to the ones that remained the same and multiply that by an experimentally determined value

The calculation assumes an inverse square relationship between light intensity and distance, but it is actually more complicated than that since light can be focused to attenuate more slowly over distance. Nonetheless, the assumption of an inverse square relationship turned out to be reasonable.

Measuring Distance, Code

  
 -(void)getDistance:(UIImage *)noLightImage 
           andLightedImage:(UIImage *)lightedImage{
    CGImageRef noLightImageRef = [noLightImage CGImage];
    NSUInteger width = CGImageGetWidth(noLightImageRef);
    NSUInteger height = CGImageGetHeight(noLightImageRef);

    CGImageRef lightedImageRef = [lightedImage CGImage];
    
    if((CGImageGetWidth(lightedImageRef)!=width)||
       (CGImageGetHeight(lightedImageRef)!=height))
    {
        hasDistance = NO;
        return;
    }
    
    
    //image data into raw data buffers
    CGColorSpaceRef colorSpace = 
                 CGColorSpaceCreateDeviceRGB();
    unsigned char *lightedRawData = (unsigned char*) 
        calloc(height * width * 4, sizeof(unsigned char));
    NSUInteger bytesPerPixel = 4;
    NSUInteger bytesPerRow = bytesPerPixel * width;
    NSUInteger bitsPerComponent = 8;
    CGContextRef context = 
        CGBitmapContextCreate(lightedRawData, width, height,
                           bitsPerComponent, bytesPerRow, 
                           colorSpace,
                           kCGImageAlphaPremultipliedLast | 
                           kCGBitmapByteOrder32Big);
    CGColorSpaceRelease(colorSpace);
    CGContextDrawImage(context, 
           CGRectMake(0, 0, width, height), 
           lightedImageRef);
    CGContextRelease(context);

    
    unsigned char *noLightRawData = (unsigned char*) 
                               calloc(height * width * 4, 
                               sizeof(unsigned char));
    context = CGBitmapContextCreate(noLightRawData, width, 
                         height,
                         bitsPerComponent, bytesPerRow, 
                         colorSpace,
                         kCGImageAlphaPremultipliedLast | 
                         kCGBitmapByteOrder32Big);
    CGColorSpaceRelease(colorSpace);
    CGContextDrawImage(context, CGRectMake(0, 0, width, 
          height),  noLightImageRef);
    CGContextRelease(context);
    
    int totalSame = 0.0;
    int totalBrighter = 0.0;
    int cutoff=40;//determined experimentally

    for(int row =0; (row+8)<height; row+=8){

        for(int column=0; column<width; column++){
            for(int rowmatrix=0; rowmatrix<8; rowmatrix++){
                
                int byteIndex = 
                    (bytesPerRow * row+rowmatrix) + 
                    (column * bytesPerPixel);
                CGFloat red   = 
                    (noLightRawData[byteIndex]     * 1.0);
                CGFloat green = 
                    (noLightRawData[byteIndex + 1] * 1.0);
                CGFloat blue  = 
                    (noLightRawData[byteIndex + 2] * 1.0);
                CGFloat noLightAverage = 
                    (red+green+blue)/3.0;

                CGFloat lightedRed   = 
                    (lightedRawData[byteIndex]  * 1.0);
                CGFloat lightedGreen = 
                    (lightedRawData[byteIndex + 1] * 1.0);
                CGFloat lightedBlue  = 
                    (lightedRawData[byteIndex + 2] * 1.0);
                CGFloat lightAverage = 
                    (lightedRed+lightedGreen+
                     lightedBlue)/3.0;
                
                if((lightAverage-noLightAverage)>cutoff){
                    totalBrighter++;
                }
                else{
                    totalSame++;
                }
            }
        }
    }
    
    free(lightedRawData);
    free(noLightRawData);

    if(totalBrighter<2000){//noise
        hasDistance = NO;
    }
    else{
        hasDistance = YES;
        //the factor of 22 was determined experimentally 
        //and is affected
        //by light level and power and alignment of LEDs
        distance = 
            (sqrt(totalSame/(float)totalBrighter))*22;
    }
    
    //use something like this when calibrating
    //distanceString = [NSString 
    //        stringWithFormat:@"same: %i brighter: %i ", 
    //totalSame, totalBrighter];
 }
  
 

There is also some choreography needed to do things in the right order. This references classes that I am not going to release, but the choreography may be useful to have. The timing of saving of frames is critical, since you need to have the LED on, but you need to avoid the automatic light level adjustment of the iDevice camera, which will cause incorrect readings of number of pixels changed.

  
 -(void)determineDistance{
    [self performSelector:@selector(turnOffLights) 
         withObject:nil afterDelay:0.1];
 }

 -(void)turnOffLights{
    [[LightCodedOutput sharedInstance] setLightPower:0];
    //delay for light level adjustment
    [self performSelector:@selector(saveNoLightPhoto) 
        withObject:nil afterDelay:1.5];
 }

 -(void)saveNoLightPhoto{
    [ImageCaptureService saveNoLightImage];
    [self performSelector:@selector(turnOnLights) 
        withObject:nil afterDelay:0.25];
 }

 -(void)turnOnLights{
    [[LightCodedOutput sharedInstance] setLightPower:10];
    //don't delay or light level adjustment kicks in
    [self performSelector:@selector(saveLightPhoto) 
        withObject:nil afterDelay:0.25];
 }

 -(void)saveLightPhoto{
    [ImageCaptureService saveLightImage];
    [self performSelector:@selector(getDistance) 
       withObject:nil afterDelay:0.1];
 }

-(void)getDistance{
    [[LightCodedOutput sharedInstance] setLightPower:0];
    [self getDistance:[ImageCaptureService getNoLightImage] 
          andLightedImage:
              [ImageCaptureService getLightImage]];
}
  
 

Next Steps

This functionality will not be included in the first release of EyesBot Driver, but will likely be incorporated in a subsequent release.

Blog

Robotics: How to Use an LED to Determine Distances

Make a VR Robot

Make a Robot controlled with EyesBot App

Simple Computer Vision with Javascript

Simple Obstacle Avoidance

Robots and Cassandra

Robotics: Controlling CIM Motors with Jaguar Motor Controllers using an Arduino

Building a Robot Controlled with an iPod - Easier Version

Simple Computer Vision with Web Services

Robot Reacting to Visual Stimulus

First Look at the MyRobots API

Calling Web Services from a Robot

Motion Detection and Tracking in C#

Building a Robot Controlled with an iPod - Part One

Building a Robot Controlled with an iPod - Part Two

Measure the Volume of Liquid in a Container using an Arduino and a Pressure Sensor

Applied Object Recognition

Robotics: How to Use an LED to Determine Distances

Communicating with an Arduino or Hardware from an iOS Device

Using NPN and PNP Transistors

Optimum Deployment of Vision Based Security Devices

Object Recognition

Applied Physical Computing

Eyesbot Company

Download EyesBot Driver Free!

EyesBot

EyesBot Company

Developers

EyesBot Blog