apf.java

机器人避障算法---人工势场法,在java的实现,采用simbad平台

import javax.vecmath.Point3d;
import javax.vecmath.Vector2d;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector3f;


import simbad.sim.*;
import simbad.demo.*;
import simbad.gui.Simbad;

/**
 * A Basic demo with camera sensor, sonars and bumpers. The robot wanders and
 * stops when it collides.
 */
public class apf extends Demo
{
	// 全局目标坐标
	Vector2d goal = new Vector2d(8,8 );
	Vector3d goal3d = new Vector3d(8, 0,8 );

	private static final double repelConstant = 10.0;// 斥力系数
	private static final double attractConstant = 30.0;// 引力系数
	private boolean debug = false;// 调试标记

	public class Robot extends Agent
	{

		RangeSensorBelt sonars,bumpers;
		LampActuator lamp;
		
		private Vector3d origin = null;

		public void initBehavior()
		{
			// nothing particular in this case
		}

		public Robot(Vector3d position, String name)
		{
			super(position, name);
			
			bumpers = RobotFactory.addBumperBeltSensor(this);			
			sonars = RobotFactory.addSonarBeltSensor(this);
			lamp = RobotFactory.addLamp(this);
			origin = position;// 起点位置

		}

		public Vector3d getVelocity()
		{
			return this.linearVelocity;
		}

		private int getQuadrant(Vector2d vector)
		{
			double x = vector.x;
			double y = vector.y;
			if (x > 0 && y > 0)// 第一象限
			{
				return 1;
			} else if (x < 0 && y > 0)// 第二象限
			{
				return 2;
			} else if (x < 0 && y < 0)// 第三象限
			{
				return 3;
			} else if (x > 0 && y < 0)// 第四象限
			{
				return 4;
			} else if (x > 0 && y == 0)// x正半轴
			{
				return -1;
			} else if (x == 0 && y > 0)// y正半轴
			{
				return -2;
			} else if (x < 0 && y == 0)// x负半轴
			{
				return -3;
			} else if (x == 0 && y < 0)// y负半轴
			{
				return -4;
			} else
			{
				return 0;
			}
		}

		private double getAngle(Vector2d v1, Vector2d v2)
		{

			double k = v1.y / v1.x;
			double y = k * v2.x;
			switch (getQuadrant(v1))
			{
			case 1:
			case 4:
			case -1:
				if (v2.y > y)
				{
					return v1.angle(v2);
				} else if (v2.y < y)
				{
					return 2 * Math.PI - v1.angle(v2);
				} else
				{
					if (v1.x * v2.x < 0)
					{
						return Math.PI;
					} else
					{
						if (debug)
							System.out.println("NO");
						return 0;
					}
				}
			case 2:
			case 3:
			case -3:
				if (v2.y > y)
				{
					return 2 * Math.PI - v1.angle(v2);
				} else if (v2.y < y)
				{
					return v1.angle(v2);
				} else
				{
					if (v1.x * v2.x < 0)
					{
						return Math.PI;
					} else
					{
						if (debug)
							System.out.println("here");
						return 0;
					}
				}
			case -2:
				int i = getQuadrant(v2);
				if (i == -4)
				{
					return Math.PI;
				} else if (i == -2 || i == -1 || i == 1 || i == 4)
				{
					return 2 * Math.PI - v1.angle(v2);
				} else
				{
					return v1.angle(v2);
				}
			case -4:
				int j = getQuadrant(v2);
				if (j == -1)
				{
					return Math.PI;
				} else if (j == -4 || j == -1 || j == 1 || j == 4)
				{
					return v1.angle(v2);
				} else
				{
					return 2 * Math.PI - v1.angle(v2);
				}
			default:
				return -1;
			}

		}

		private Vector2d transform(Vector2d v, Vector2d point)
		{
			Vector2d global = new Vector2d(1, 0);
			double alfa = getAngle(global, v);
			double beta = getAngle(point, v);

			double k1 = Math.cos(alfa + beta) / Math.cos(beta);
			double k2 = Math.sin(alfa + beta) / Math.sin(beta);

			double x = point.x * k1;
			double y = point.y * k2;

			return new Vector2d(x, y);

		}

		private double repelForce(double distance, double range)
		{
			double force = 0;
			Point3d p = new Point3d();
			getCoords(p);
			Vector2d pos = new Vector2d(p.z, p.x);
			Vector2d toGoal = new Vector2d((goal.x - pos.x),
					(goal.y - pos.y));
			double disGoal = toGoal.length();
			double n=0.5;
			if (distance <= range)
			{
				force = repelConstant / (distance * distance);
			}

			return force;
		}

		private double attractForce(double distance)
		{
			double force = attractConstant * distance;
			return force;
		}

		private boolean checkGoal()
		{
			// get the current postion of the robot
			Point3d currentPos = new Point3d();
			getCoords(currentPos);
			Point3d goalPos = new Point3d(goal3d.x, goal3d.y, goal3d.z);

			if (currentPos.distance(goalPos) <= 0.5) // 如果当前距离目标点小于0.5那么即认为是到达
			{
				return true;
			} else
			{
				return false;
			}
		}
		
		
		public void performBehavior()
		{
			
			
			// 为了防止智能体剧烈晃动，每10帧计算一次受力
			if (getCounter() % 10 == 0)
			{
				// get the velocity of the robot
				Vector3d velocity = getVelocity();

				// get the moving direction vector in the plane
				Vector2d direct = new Vector2d(velocity.z, velocity.x);

				// get the postion of the robot in the plane
				Point3d p = new Point3d();
				getCoords(p);
				Vector2d pos = new Vector2d(p.z, p.x);

				// get the measurements of the three sonars at the front
				double d0 = sonars.getMeasurement(0);// front声纳，正前方
				double d1 = sonars.getMeasurement(1);// frontleft声纳，左前方
				double d2 = sonars.getMeasurement(8);// frontright声纳，右前方

				// compute the three repelling forces using the three
				// measurements
				double rf0 = repelForce(d0, 2.0);
				double rf1 = repelForce(d1, 2.0);
				double rf2 = repelForce(d2, 2.0);

				// compute the composition of the three repelling forces in the
				// local reference frame
				double k1 = Math.cos(2 * Math.PI / 9);
				double k2 = Math.sin(2 * Math.PI / 9);
				Vector2d vf0 = new Vector2d(0 - rf0, 0);
				Vector2d vf1 = new Vector2d((0 - rf1 * k1), (0 - rf1 * k2));
				Vector2d vf2 = new Vector2d((rf2 * k1), (rf2 * k2));
				Vector2d composition = new Vector2d();
				composition.setX(vf0.x + vf1.x + vf2.x);
				composition.setY(vf0.y + vf1.y + vf2.y);

				if (debug)
					System.out.println("(" + composition.x + ","
							+ composition.y);

				// transform the composition of the repelling forces to the
				// global coordinate
				Vector2d repelForceVector = transform(direct, composition);

				// compute the attracting force from the goal
				Vector2d toGoal = new Vector2d((goal.x - pos.x),
					(goal.y - pos.y));
				double disGoal = toGoal.length();
				if (debug)
					System.out.println("distance to goal:" + disGoal);
				double goalForce = attractForce(disGoal);

				if (debug)
					System.out.println("attract force from goal:" + goalForce);
				Vector2d goalForceVector = new Vector2d(
					(goalForce * toGoal.x / disGoal),
					(goalForce * toGoal.y / disGoal));
				Vector2d originForceVector = new Vector2d(origin.x, origin.z);

				// get the composition of all forces
				double x = repelForceVector.x + goalForceVector.x;
				double y = repelForceVector.y + goalForceVector.y;

				Vector2d allForces = new Vector2d(x, y);
				if (debug)
				{
					System.out.println("total force(" + allForces.x + ","
							+ allForces.y + ")");
					System.out.println("force direct(" + direct.x + ","
							+ direct.y + ")");
				}
				// decide the direction that the robot should move according to
				// the force
				double angle = getAngle(direct, allForces);

				// System.out.println(0.0005*Math.random());
				// angle=angle*(1+0.1*Math.random());

				if (debug)
					System.out.println("angle:" + angle);

				// 判断转动方向
				if (angle < Math.PI)
				{
					setRotationalVelocity(angle);
				} else if (angle > Math.PI)
				{
					setRotationalVelocity((angle - 2 * Math.PI));
				}

				// check if the robot has reached the goal
				if (checkGoal())
				{
					// 到达目标点，停止运动
					setTranslationalVelocity(0);
					setRotationalVelocity(0);
					lamp.setOn(true);
					return;
				} else
				{
					lamp.setOn(false);
					setTranslationalVelocity(0.5);
				}

				// 检测是否碰撞
				if (bumpers.oneHasHit())
				{
					lamp.setBlink(true);					
					// reads the three front quadrants
					double left = sonars.getFrontLeftQuadrantMeasurement();
					double right = sonars.getFrontRightQuadrantMeasurement();
					double front = sonars.getFrontQuadrantMeasurement();
					// if obstacle near
					if ((front < 0.7) || (left < 0.7) || (right < 0.7))
					{
						if (left < right)
						{
							setRotationalVelocity(-1 - (0.1 * Math.random()));// 随机向右转
						} else
						{
							setRotationalVelocity(1 - (0.1 * Math.random()));// 随机向左转
						}
						setTranslationalVelocity(0);
					}
				}
				else
					lamp.setBlink(false);
			}
		}
	}

	static public class obs extends Agent{
    	
    	RangeSensorBelt bumpers;
    	
    	protected boolean movability;
    	
    	public obs(Vector3d position, String name) {
			super(position, name);
			
			bumpers = RobotFactory.addBumperBeltSensor(this);
			
		}
    	
    	void setMovability(boolean bool){
    		movability=bool;
    	}
    	
        /** This method is called by the simulator engine on reset. */
        public void initBehavior() {
            // nothing particular in this case
        	//this.setMovability(true);
        	this.setMovability(false);
        }

        /** This method is call cyclically (20 times per second)  by the simulator engine. */
        public void performBehavior() {
        	if(this.movability){
        		if(bumpers.oneHasHit()){
        			double r=Math.random();        	       		
                	if(r>0.5)
                		setRotationalVelocity(-0.5+ 0.1 * r);
                	else
                		setRotationalVelocity(0.5-0.1 * r );
        		}
        		else{
        			double r=Math.PI;
        			if(r>0.5){
        				this.setTranslationalVelocity(0.08*r);
        				this.setRotationalVelocity(0.5-0.1*Math.PI);
        				}
        			else{
        				this.setTranslationalVelocity(-0.08*r);
        				this.setRotationalVelocity(-0.5+0.1*Math.PI);
        			}
        		}
        	}
        }
    }
    
	
	/**
	 * 构造函数
	 */
	public apf()
	{
		//Env-boarder
    	Wall w1 = new Wall(new Vector3d(10, 0, 0), 20, 1, this);
        w1.rotate90(1);
        add(w1);
        Wall w2 = new Wall(new Vector3d(-10, 0, 0), 20, 1, this);
        w2.rotate90(1);
        add(w2);
        Wall w3 = new Wall(new Vector3d(0, 0, 10), 20, 1, this);
        add(w3);
        Wall w4 = new Wall(new Vector3d(0, 0, -10), 20, 1, this);
        add(w4);
        
        
        //wall
        Wall w5 = new Wall(new Vector3d(6, 0, 0), 5, 1, this);
        add(w5);
        Wall w6 = new Wall(new Vector3d(-6, 0,0), 5, 1, this);
        add(w6);
        Wall w7 = new Wall(new Vector3d(-7, 0, 7), 2, 1, this);
        w7.rotate90(1);
        add(w7);
        Wall w8 = new Wall(new Vector3d(7, 0, -7), 2, 1, this);
        w8.rotate90(1);
        add(w8);
        Wall w9 = new Wall(new Vector3d(-7, 0, -7), 2, 1, this);
        w9.rotate90(1);
        add(w9);
        Wall w10 = new Wall(new Vector3d(7, 0, 7), 2, 1, this);
        w10.rotate90(1);
        add(w10);
        
        
        //box
        Box b1 = new Box(new Vector3d(0,0,0), new Vector3f(3, 1, 3),this);                      
        add(b1);
        Box b2 = new Box(new Vector3d(-4,0,-4), new Vector3f(1, 3, 1),this);
        add(b2);
        Box b3 = new Box(new Vector3d(4,0,4), new Vector3f(1, 3, 1),this);
        add(b3);
        Box b4 = new Box(new Vector3d(-4,0,4), new Vector3f(1, 3, 1),this);
        add (b4);
        Box b5 = new Box(new Vector3d(4,0,-4), new Vector3f(1, 3, 1),this);
        add(b5);
        
        
        /*
        //u
        Wall a=new Wall(new Vector3d(0,0,0),2,1,this);
        Wall b=new Wall(new Vector3d(0,0,1),2,1,this);
        Wall c=new Wall(new Vector3d(0,0,-1),2,1,this);
        
        
        a.rotate90(1);
        add(a);
        add(b);
        add(c);
        */
        /*
        //arch
        Arch a1=new Arch(new Vector3d(0,0,6),this);
        a1.rotate90(2);
        add(a1);
        Arch a2=new Arch(new Vector3d(0,0,-6),this);
        a2.rotate90(4);
        add(a2);
        */
        
        
        add(new Robot(new Vector3d(-8, 0, -8), "My Robot"));
        /*
        add(new obs(new Vector3d(0, 0, 0), "obs 1"));
        add(new obs(new Vector3d(-5, 0, -5), "obs 2"));
        add(new obs(new Vector3d(5, 0, 5), "obs 3"));
        add(new obs(new Vector3d(1, 0, 3), "obs 4"));
        add(new obs(new Vector3d(-3, 0, -4), "obs 5"));
        add(new obs(new Vector3d(4, 0, 3), "obs 6"));
        add(new obs(new Vector3d(-3, 0, -2), "obs 7"));
        add(new obs(new Vector3d(5, 0, -3), "obs 8"));
        add(new obs(new Vector3d(7, 0, 6), "obs 9"));
        */


	}

	public static void main(String[] args)
	{
		// request antialising
		System.setProperty("j3d.implicitAntialiasing", "true");
		// create Simbad instance with given environment
		Simbad frame = new Simbad(new apf(), false);
	}
}