基于Simbad平台的机器人避障模拟与实现

在机器人软件平台上建立一个包含若干个静止障碍物和运动障碍物的仿真环境，设定机器人的起始点和终点后，机器人能够规划出一条从起始点到目标点的安全路径。查阅相关路径规划算法，实现一种以上算法并相互比较。要求给出源代码、试验结果并且进行演示。

package myrobot;
 
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; 
  
public class MyRobot extends Demo 
{ 
	// 全局目标坐标 
	Vector2d goal = new Vector2d（88 ）; 
	Vector3d goal3d = new Vector3d（8 08）; 
 
	private static final double repelConstant = 100.0;// 斥力系数 
	private static final double attractConstant = 30.0;// 引力系数 
	private boolean debug = false;// 调试标记 
 
	public class Robot extends Agent 
	{ 
 
		RangeSensorBelt sonarsbumpers; 
		LampActuator lamp; 
		 
		private Vector3d origin = null; 
 
		public void initBehavior（） 
		{  
		} 
 
		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）; 
	

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2013-01-07 18:30  MyRobot\
     文件        3716  2013-01-07 18:27  MyRobot\build.xml
     文件          85  2013-01-07 18:27  MyRobot\manifest.mf
     目录           0  2013-01-07 18:27  MyRobot\nbproject\
     文件       78029  2013-01-07 18:27  MyRobot\nbproject\build-impl.xml
     文件         475  2013-01-07 18:27  MyRobot\nbproject\genfiles.properties
     文件        2332  2013-01-07 18:27  MyRobot\nbproject\project.properties
     文件         515  2013-01-07 18:27  MyRobot\nbproject\project.xml
     目录           0  2013-01-07 18:28  MyRobot\src\
     目录           0  2013-01-07 18:27  MyRobot\src\myrobot\
     文件       11506  2013-01-07 19:09  MyRobot\src\myrobot\MyRobot.java
     目录           0  2013-01-07 18:28  MyRobot\src\simbad\
     目录           0  2013-01-07 18:28  MyRobot\src\simbad\demo\
     文件        4138  2013-01-07 18:28  MyRobot\src\simbad\demo\AvoidersDemo.java
     文件        3106  2013-01-07 18:28  MyRobot\src\simbad\demo\baseDemo.java
     文件        4078  2013-01-07 18:28  MyRobot\src\simbad\demo\blinkingLampDemo.java
     文件        4149  2013-01-07 18:28  MyRobot\src\simbad\demo\BumpersDemo.java
     文件        1190  2013-01-07 18:28  MyRobot\src\simbad\demo\Demo.java
     文件        2706  2013-01-07 18:28  MyRobot\src\simbad\demo\DemoManager.java
     文件        3440  2013-01-07 18:28  MyRobot\src\simbad\demo\DifferentialKinematicDemo.java
     文件        4506  2013-01-07 18:28  MyRobot\src\simbad\demo\ImagerDemo.java
     文件        2103  2013-01-07 18:28  MyRobot\src\simbad\demo\KheperaDemo.java
     文件        3509  2013-01-07 18:28  MyRobot\src\simbad\demo\LightSearchDemo.java
     文件        2737  2013-01-07 18:28  MyRobot\src\simbad\demo\PickCherriesDemo.java
     文件        3272  2013-01-07 18:28  MyRobot\src\simbad\demo\PushBallsDemo.java
     文件        1895  2013-01-07 18:28  MyRobot\src\simbad\demo\SimplestDemo.java
     文件        4464  2013-01-07 18:28  MyRobot\src\simbad\demo\SingleAvoiderDemo.java
     目录           0  2013-01-07 18:28  MyRobot\src\simbad\gui\
     文件        2444  2013-01-07 18:28  MyRobot\src\simbad\gui\AgentFollower.java
     文件        5673  2013-01-07 18:28  MyRobot\src\simbad\gui\AgentInspector.java
     文件        3534  2013-01-07 18:28  MyRobot\src\simbad\gui\Console.java
............此处省略50个文件信息