Laser sensor and localization

[remzaman]

That's brill. Grats!

[esmetaman]

Hi Andy,

I found another doc about this matter:
cswww.essex.ac.uk/staff/hhu/Papers/Journal-IR27-6.pdf

Imagine that you have a robot with a pose (20,30,0) and it detects 3 landmarks with angles:
30,50,70.



We know that the distance from landmark1 to landmark2 = 40cm and landmark2 to landmark3 = 50

¿How to calculate distances?

[gloomyandy]

The details of how to calculate the position are all given in the paper I posted earlier:

http://repositorium.sdum.uminho.pt/bits ... 003328.pdf

There is a step by step sequence of how to perform the calculation for any position of the robot or beacons.

Andy

[gloomyandy]

I've been asked a few time for the code that performs localization. So here it is. This code does the modulation of the laser, scans the platform over approx 180 degrees, performs peak detection looking for 3 beacons that are at known locations. Having identified the beacons it uses the obtained angles to calculate the pose. The code is not pretty, I put it together very quickly to test things out. Use it or ignore it as you see fit.


Even though I've posted my code I would encourage anyone trying to do this sort of localization to read the paper above. Try and work out your solution to implementing the algorithm. Playing around with the actual calculations and feeding in different examples of position and angles really gives you a feel for how it works.

Code: Select all

class BeaconLocator
{
   private static int ONEDEGREE = 56;
   private Motor motor=null;
   private LightSensor ls;
   private int baseTacho;
   private static final int SCAN_ANGLE = 40;
   private static final int HIT_THRESHOLD = 10;
   private ArrayList<Double> beaconAngles = new ArrayList<Double>();
   
   public BeaconLocator(SensorPort port, Motor motor)
   {
      ls = new LightSensor(port);
      this.motor = motor;
      baseTacho = motor.getTachoCount();
      reset();
   }

   public void reset()
   {
      ls.setFloodlight(false);
      stop();
      motor.setSpeed(900);
      motor.rotateTo(baseTacho);
   }

   public void stop()
   {
      motor.stop();
   }


   public double getAngle()
   {
      return ((double)motor.getTachoCount() - baseTacho)/ONEDEGREE;
   }

   public void rotateTo(int angle)
   {
      motor.rotateTo(angle*ONEDEGREE, true);
   }
   public void rotateTo(double angle)
   {
      motor.rotateTo((int)Math.round(angle*ONEDEGREE), true);
   }

    int ping()
    {
        Delay.msDelay(10);
        int val1 = ls.readNormalizedValue();
       
        for(int i = 0; i < 10; i++)
        {
            int newVal = ls.readNormalizedValue();
            if (newVal > val1) val1 = newVal;
            Delay.msDelay(1);
        }
        ls.setFloodlight(true);
        Delay.msDelay(10);
        int val2 = ls.readNormalizedValue();

        for(int i = 0; i < 10; i++)
        {
            int newVal = ls.readNormalizedValue();
            if (newVal > val2) val2 = newVal;
            Delay.msDelay(1);
        }
        ls.setFloodlight(false);
        return val2 - val1;
       
    }

    static final int LOW_THRESHOLD = 2;

   public ArrayList<Double> locate()
   {
        beaconAngles.clear();
        double peakAngle = -360;
        int peakValue = Integer.MIN_VALUE;
        int lowCnt = 0;
        int peakTotal = 0;
        int peakCnt = 0;
        motor.setSpeed(900);
        rotateTo(-SCAN_ANGLE);
        while (motor.isMoving())
            Delay.msDelay(1);
        motor.setSpeed(250);
        rotateTo(SCAN_ANGLE);
        int sval = 0;
        int svar = 16;
        int ssval = 0;
        int ssvar = 0;
        while(motor.isMoving())
        {
            int value = ping()*8;
           
            //RConsole.println(" " + getAngle()+ " " + (float)value/8 + " s " + (float)sval/8 + " v " + (float)svar/8);
            // detect peak values
            if (lowCnt > 0)
            {
                // We are in peak detection mode
                //if (value > sval + 2*svar)
                //{
                    // Do we have a genuine peak?
                    if (value > peakValue)
                    {
                        peakAngle = getAngle();
                        peakValue = value;
                    }
                if (value > ssval + 2*ssvar)
                {
                    lowCnt = LOW_THRESHOLD;
                    peakCnt++;
                    peakTotal += value;
                }
                //}
                else if (--lowCnt <= 0)
                {
                    // end of peak detection
                    boolean detected = (peakTotal > (ssval + 4*ssvar)*peakCnt) && peakCnt > 1;
                    //RConsole.println("***** end " + detected + " ang " + peakAngle + " average " + (float)peakTotal/peakCnt/8);
                    if (detected)
                    {
                        if (peakAngle < 0) peakAngle = peakAngle + 360;
                        beaconAngles.add(peakAngle);
                        svar = ssvar;
                        sval = ssval;
                        Sound.beep();
                    }
                    peakAngle = -360;
                    lowCnt = 0;
                }
            }
            else if (value > sval + svar*2)
            {
                // Start peak detection
                lowCnt = LOW_THRESHOLD;
                ssvar = svar;
                ssval = sval;
                peakValue = value;
                peakCnt = 1;
                peakAngle = getAngle();
                peakTotal = value;
                //RConsole.println("***** Start");
            }
            // Adjust background readings
            int delta = (value - sval)/8;
            sval += delta;
            if (delta < 0) delta = -delta;
            delta -= svar/8;
            svar += delta;
        }
        //if (peakAngle > -180)
       //     beaconAngles.add(peakAngle);
        motor.setSpeed(900);
        rotateTo(0);
        while (motor.isMoving())
            Delay.msDelay(1);
        return beaconAngles;
   }

    public void showBeacons(ArrayList<Double> beacons)
    {
        motor.setSpeed(900);
        rotateTo(-SCAN_ANGLE);
        while (motor.isMoving())
            Delay.msDelay(1);
        for(Double d : beacons)
        {
            if (d > 180) d -= 360;
            rotateTo(d);
            while (motor.isMoving())
                Delay.msDelay(1);
            ls.setFloodlight(true);
            Delay.msDelay(2000);
            ls.setFloodlight(false);
        }
        rotateTo(0);
        while (motor.isMoving())
            Delay.msDelay(1);
    }


    static double sin(double d)
    {
        return Math.sin(Math.toRadians(d));
    }

    static double cos(double d)
    {
        return Math.cos(Math.toRadians(d));
    }

    static double atan(double r)
    {
        return Math.toDegrees(Math.atan(r));
    }

    public Pose calcPose(double a1, double a2, double a3)
    {
        double x1 = 0;
        double y1 = 2000;
        double x2 = 500;
        double y2 = 2000;
        double x3 = 1200;
        double y3 = 2000;
        double Y1 = a1;
        double Y2 = a2;
        double Y3 = a3;

        double Y12 = Y2 - Y1;
        if (Y1 > Y2) Y12 = 360 + Y12;
        double Y31 = Y1 - Y3;
        if (Y3 > Y1) Y31 = 360 + Y31;
        double L12 = x2-x1;
        double L31 = x3-x1;
        double T = 180;
        double R = 180;
        double Y = R - Y31;
        double L1;
        double L2;
        double xR;
        double yR;
        double aR;

        double t = atan((sin(Y12)*(L12*sin(Y31) - L31*sin(Y)))/(L31*sin(Y12)*cos(Y) - L12*cos(Y12)*sin(Y31)));
        //System.out.println("t = " + t);
       // System.out.println("Y = " + Y);
        if (Y < 180 && t < 0) t = t + 180;
        if (Y12 > 180 && t > 0) t = t - 180;
        //System.out.println("t = " + t);
        //System.out.println("Y = " + Y);
        if (Math.abs(sin(Y12)) > Math.abs(Y31))
            L1 = (L12*sin(t + Y12))/sin(Y12);
        else
            L1 = (L31*sin(t + R - Y31))/sin(Y31);
        xR = x1 - L1*cos(T+t);
        yR = y1 - L1*sin(T+t);
        aR = T + t - Y1;
        //System.out.println("xR = " + xR + " yR = " + yR + " aR = " + aR);
        if (aR <= -180) aR = aR + 360;
        if (aR > 180) aR = aR - 360;
        //System.out.println("xR = " + xR + " yR = " + yR + " aR = " + aR);
        return new Pose((float)xR, (float)yR, (float)aR);

    }
   
    public Pose getPose()
    {
        ArrayList<Double> beacons = locate();
        RConsole.println("Located " + beacons.size());
        for(Double d : beacons)
            RConsole.println("Beacon " + d);
        if (beacons.size() == 3)
        {
            Pose p = calcPose(beacons.get(2), beacons.get(1), beacons.get(0));
            RConsole.println("pose: " + p.getX() + ", " + p.getY() + " : " + p.getHeading());
            return p;
        }
        return null;
    }
}


[clintonb]

That is such an awesome project, Andy!

Thanks for sharing.
Clinton

[esmetaman]

Many thanks for the example. It is very educative to learn.

Nice job

Cheers