Laser sensor and localization

Post your NXJ projects, project ideas, etc here!

Moderators: 99jonathan, roger, imaqine

Postby remzaman » Tue Sep 28, 2010 7:31 pm

That's brill. Grats!
remzaman
New User
 
Posts: 3
Joined: Tue Sep 28, 2010 5:18 pm

Postby esmetaman » Sun Jan 30, 2011 7:02 pm

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.

Image

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

¿How to calculate distances?
Juan Antonio Breña Moral
http://www.juanantonio.info/lejos-ebook/
http://www.roboticaenlaescuela.es
User avatar
esmetaman
Advanced Member
 
Posts: 236
Joined: Wed Sep 13, 2006 12:16 am
Location: Madrid, Spain

Postby gloomyandy » Sun Jan 30, 2011 8:56 pm

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
User avatar
gloomyandy
leJOS Team Member
 
Posts: 3642
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Postby gloomyandy » Mon Jan 31, 2011 8:40 pm

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;
    }
}
User avatar
gloomyandy
leJOS Team Member
 
Posts: 3642
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Postby clintonb » Tue Feb 01, 2011 3:51 am

That is such an awesome project, Andy!

Thanks for sharing.
Clinton
clintonb
Active User
 
Posts: 117
Joined: Fri May 28, 2010 1:44 am
Location: Cardston, Alberta, Canada

Postby esmetaman » Tue Feb 01, 2011 9:23 pm

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

Nice job

Cheers
Juan Antonio Breña Moral
http://www.juanantonio.info/lejos-ebook/
http://www.roboticaenlaescuela.es
User avatar
esmetaman
Advanced Member
 
Posts: 236
Joined: Wed Sep 13, 2006 12:16 am
Location: Madrid, Spain

Previous

Return to NXJ Projects

Who is online

Users browsing this forum: Baidu [Spider] and 1 guest

cron
more stuff