Java 机器人编程入门手册（三）
原文：Beginning Robotics Programming in Java with LEGO Mindstorms
七、爬山搜索及其在 Lego Mindstorms 中的实现
正如在第五章中向您介绍深度优先搜索(DFS)算法和在第六章中向您介绍广度优先搜索算法一样，在本章中，您将学习启发式搜索策略背后的基本思想以及如何实现爬山算法，这是 leJOS EV3 中最典型的启发式方法之一。具体而言，本章将涵盖以下主题:
- 一种新的爬山搜索算法，可用于构建任意树结构。
- 在基于 leJOS 的机器人系统中应用和集成所提出的爬山算法用于定位和路径规划，这增强了 leJOS 系统中现有的寻路方法。
启发式搜索简介
问题求解是许多人工智能相关应用中的基本问题之一。通常，有两类问题。第一个问题可以通过使用某种确定性过程来解决，例如绝对值的计算，例如根据坐标计算两个节点之间的距离。对于计算机来说，解决这类问题非常容易，因为它们只需将问题转化为一个方程，然后就可以执行了。然而，第二类问题并不总是导致直接确定的解决方案。这种情况在现实世界中经常发生，很多问题只有通过使用搜索策略来寻找解决方案才能得到解决，比如广度优先搜索或深度优先搜索，你已经在第 5 和 6 章看到了。
随着本章的深入，您将了解启发式搜索方法。通过下面的例子，你将了解到人工智能搜索策略中使用的一些表示和术语:
假设你在学校大楼的某个地方丢了一本书。该建筑如图 7-1 所示。
图 7-1。
Building topology
你正站在“你”街区的走廊前面。当你开始搜索时，你检查 R8 房间。没有找到你的书，你回到走廊，然后检查房间 R7。没有找到你的书，你又回到走廊，等等。通过检查每一个房间，并来回回到走廊，你终于在 R2 房间找到了你的书。这样的场景可以很容易地转换成如下图 7-2 所示的图形:
图 7-2。
Graphic representation of your search strategy
正如你在第 5 和 6 章中看到的，用图形格式表示搜索问题是有益的，因为它提供了一种方便的方式来描述如何找到解决方案。纵观本书，在介绍 AI 搜索算法时，你会发现使用了以下术语:
- 节点 A 离散点:例如图 7-2 中的走廊。
- 终端节点:路径结束的节点:例如图 7-2 中的房间 R8。
- 图中所有节点的集合。
- 搜索要到达的节点。
- 启发式实用函数，用于确定任何特定节点是否是比另一个更好的下一个选择。
- 解决方案:一组连接并指向目的地的节点。
在上面的丢书场景中，大楼中的每个教室都被称为一个节点。整个建筑，包括八个教室，都是搜索空间。结果，目标是 R2 房间。最后，解决方案路径如图 7-2 虚线箭头所示。教室 R1、R2、R3、R4、R5、R6、R7 和 R8 是终端节点，因为它们不通向任何地方。试探法在图中没有表示。相反，它们是你可以用来帮助你更好地找到一条道路的技术。
鉴于上面的例子，你可能认为寻找解决方案一点也不难；也就是说，您只需从起点开始，然后逐个搜索节点，以达到您的目标。只有在上面丢书的例子这种极其简单的情况下才成立，因为搜索空间太小了(也就是只有八个节点，你最多要尝试八次才能找到解)。然而，在现实世界中，搜索空间通常非常大。例如，想想新罕布什尔州有多少个城市，而整个美国有多少个城市？随着搜索空间的增长，到达目标的可能路径的数量也在增长。此外，当向搜索空间添加另一个节点时，一个重要的问题是，它通常会添加多条路径，因此不是线性的。换句话说，当搜索空间变大时，达到目标的潜在途径的数量会以非线性方式增加。
在非线性情况下，可能路径的数量会很快变得非常大。例如，考虑您可以用来排列三个对象 A、B 和 c 的方法的数量。总共六种可能的排列如下:
| A | B | C || A | C | B |
| B | C | A |
| B | A | C |
| C | B | A |
| C | A | B |
当你再增加一个对象时，你会看到可能的排列总数是 7，因为 N 个对象可以排列的方法数等于 N！(N 阶乘)。因此，如果你有四个对象要排列，那么就会有四个！排列(即 24 种排列)。对于五个对象，该数目变成 120 个排列，而对于六个对象，该数目增加到 720 个排列。对于 100 个对象，可能的排列数量是巨大的:93326215443944152681 69923885626670049071 59682643816214685929 63895217599993229915 60894146397615651828 62536979208272237582 51185210916864000000 000000000000000000。在这种情况下，检查所有节点的穷举搜索将不起作用，因为它消耗太多时间和太多计算资源。因此，基于人工智能的搜索技术对于搜索解决方案至关重要。三种最基本的技术是深度优先搜索、宽度优先搜索和爬山搜索。
比较和评估各种基于人工智能的搜索技术的性能可能非常复杂。在本书中，我们主要关注两个衡量标准:(1)找到解决方案有多快，以及(2)解决方案有多好。当您试图以最小的努力找到一个解决方案时，第一个衡量标准——即找到一个解决方案的速度——尤其重要，它通常受到搜索空间大小和在找到解决方案的过程中实际遍历的节点数量的影响。这是因为从死胡同回溯是耗时的，并且你想要一个很少回溯的搜索。在其他情况下，解决方案的质量更重要。在基于人工智能的搜索中，找到最佳解决方案和找到好的解决方案是有区别的。寻找最佳解决方案需要彻底的搜索，因为这是知道已经找到最佳解决方案或者已经实现全局优化的唯一方式。另一方面，找到一个好的解决方案意味着找到一个在定义的约束集内的解决方案，但是如果存在一个更好的解决方案，并且您获得的可能是一个局部最优的解决方案，这并不重要。
正如你在第五章中看到的，当第一次尝试排除回溯时，深度优先方法可以找到一个好的解决方案。然而，当我们按照第五章的图 5-2 所示重新组织图形时，寻找解决方案可能会涉及大量的回溯。因此，在第五章中介绍的例子的结果不能推广。此外，当探索一个特别长的末端无解的分支时，深度优先搜索的性能会相当差。在这种情况下，深度优先搜索会浪费探索这个分支和回溯到目标的时间。
在第六章举例说明的例子中，广度优先搜索表现非常好，找到了一个合理的解决方案。类似于深度优先搜索，这种解决方案不能通用化，因为要找到的第一条路径取决于节点信息的实际物理组织。比较深度优先搜索和广度优先搜索，您可以在相同的搜索空间中找到不同的路径。当目标不太深入搜索空间时，广度优先搜索效果很好。然而，当目标位于几层深处时，它的效果很差。在这种情况下，广度优先搜索会在回溯过程中耗费大量精力。
基于以上所述，断言一种搜索方法总是优于另一种方法是不准确的。在许多情况下，定义问题的方式可以帮助您选择合适的搜索方法。正如您在第 5 和 6 章中所了解到的，深度优先搜索方法和广度优先搜索方法都不试图对搜索空间中的一个节点是否比另一个节点更接近目标做出任何合理的猜测。相反，这两种搜索策略只是使用指定的模式从一个节点移动到下一个节点，直到最终达到目标。为了提高搜索更快达到目标的概率，您需要在搜索算法中添加启发式功能。
试探法只是增加搜索向正确方向前进的可能性的规则。例如，假设你在新罕布什尔州的白山迷路了。你仍然想爬上山顶，而且你知道在山顶上非常寒冷和多风。白山上的树林非常茂密，以至于你看不到远处。此外，树太大了，不能爬上去四处看看。然而，你知道峰顶的温度会比你现在的位置更冷，所以当你接近峰顶时，你会开始“感觉”到它，因为它变得越来越冷。
你从上山开始。你知道你正朝着正确的方向前进，因为你正沿着一条每走一步都感觉更冷的道路前进。随着你的移动，你可以感觉到温度在下降。最后，你到达了山顶。在这种情况下，用于寻找顶点的启发式信息并不能保证成功。然而，它增加了早期成功的可能性，引导你快速达到目标。
启发式搜索的基本思想是，你试着把注意力集中在那些似乎能让你更接近目标的路径上，而不是尝试所有可能的搜索路径。即使你不能确定你真的接近了你的目标，你也可以做出一个很好的猜测，启发式方法可以帮助你做出这个猜测。
在进行启发式搜索时，您需要一个效用函数，它根据您与目标状态的接近程度对搜索树中的一个节点进行评分。虽然这只是一个猜测，但它仍然有助于最大化或最小化某些约束。例如，当您应用 GPS 来设置从起点到终点的路线时，您可能希望最小化两个可能的约束。第一个是时间更快，第二个是距离最短。最短的路线不一定意味着最快的时间，反之亦然。在本章的例子中，我们将重点放在基于深度优先搜索框架的爬山搜索上，其目的是最小化称为连接数的约束。
爬山搜索概述
在爬山中，最基本的想法是朝着一个比现在更好的状态前进。因此，如果你在 A 镇，你需要到达 B 镇和 C 镇(你的最终目标是 D 镇)，那么如果 B 镇或 C 镇看起来比 A 镇更靠近 D 镇，你应该采取行动。在最陡的爬坡中，爬山搜索总是会使你的下一个州成为你当前州的最佳继任者，只有当继任者比你当前州更好时，它才会采取行动。这可以描述如下:
- 从当前状态(初始状态)开始。
- 直到当前状态=目标状态，或者当前状态没有变化，执行以下操作:
- 获取当前状态的后继者，并使用效用函数给每个后继者分配一个分数。
- 如果其中一个后继者具有比当前状态更好的分数，则将新的当前状态设置为具有最佳分数的后继者。
- 由于该算法不试图彻底测试每个节点和路径，所以除了当前状态之外，您不必维护节点。
- 当没有比当前状态本身更好的当前状态的后继者时，爬山终止。
接下来，让我们看看如何使用爬山搜索方法来解决一个寻路问题，该问题可以应用于从起始城市导航到目的城市时的 GPS 系统。假设您想使用爬山搜索算法以最少的节点数从城市 A(例如，纽约州纽约市)开车到 E(例如，马萨诸塞州波士顿)，如图 7-3 所示。假设我们使用笛卡尔坐标系如下:
- A 处的坐标是(0，0)。
- B 点的坐标是(-10，20)。
- C 点的坐标是(-15，20)。
- D 点的坐标是(0，20)。
- E 点的坐标是(0，30)。
- F 点的坐标是(-15，30)。
- 在 G 点的坐标是(10，20)。
- H 点的坐标是(10，10)。
图 7-3。
Routes in between two cities
根据下面的路线，为你制定一个计划，从 A 点出发，使用爬山搜索策略到达 E 点。
以下程序可用于通过使用爬山搜索算法为您自动安排旅行计划找到一条路径:
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch7p1_main.java
//Driver class to set up map using ch7p1_GraphNode, ch7p1_Link, and //ch7p1_Graph classes.
//Calls a hill-climbing search in ch7p1_Graph class to create //navigation path from a start
//and end node
//******************************************************************************************
public class ch7p1_main {
public static void main(String[] args) {
// These objects used to define what your graph looks // like
ch7p1_Graph searchGraph = new ch7p1_Graph();
ch7p1_GraphNode A, B, C, D, E, F, G, H;
ch7p1_Link AB, AC, AD, BD, CB, CE, CF, DE, DH, FE, GE, HG;
// define each node
A = new ch7p1_GraphNode("A", 0, 0);
B = new ch7p1_GraphNode("B", -10, 20);
C = new ch7p1_GraphNode("C", -15, 20);
D = new ch7p1_GraphNode("D", 0, 20);
E = new ch7p1_GraphNode("E", 0, 30);
F = new ch7p1_GraphNode("F", -15, 30);
G = new ch7p1_GraphNode("G", 10, 20);
H = new ch7p1_GraphNode("H", 10, 10);
// define which GraphNodes are connected
AB = new ch7p1_Link(A, B, dist(A.xLocation, A.yLocation, B.xLocation,
B.yLocation));
AC = new ch7p1_Link(A, C, dist(A.xLocation, A.yLocation, C.xLocation,
C.yLocation));
AD = new ch7p1_Link(A, D, dist(A.xLocation, A.yLocation, D.xLocation,
D.yLocation));
BD = new ch7p1_Link(B, D, dist(B.xLocation, B.yLocation, D.xLocation,
D.yLocation));
CB = new ch7p1_Link(C, B, dist(C.xLocation, C.yLocation, B.xLocation,
B.yLocation));
CE = new ch7p1_Link(C, E, dist(C.xLocation, C.yLocation, E.xLocation,
E.yLocation));
CF = new ch7p1_Link(C, F, dist(C.xLocation, C.yLocation, F.xLocation,
F.yLocation));
DE = new ch7p1_Link(D, E, dist(D.xLocation, D.yLocation, E.xLocation,
E.yLocation));
DH = new ch7p1_Link(D, H, dist(D.xLocation, D.yLocation, H.xLocation,
H.yLocation));
FE = new ch7p1_Link(F, E, dist(F.xLocation, F.yLocation, E.xLocation,
E.yLocation));
GE = new ch7p1_Link(G, E, dist(G.xLocation, G.yLocation, E.xLocation,
E.yLocation));
HG = new ch7p1_Link(H, G, dist(H.xLocation, H.yLocation, G.xLocation,
G.yLocation));
// add all nodes and links to your graph object
searchGraph.addNode(A);
searchGraph.addNode(B);
searchGraph.addNode(C);
searchGraph.addNode(D);
searchGraph.addNode(E);
searchGraph.addNode(F);
searchGraph.addNode(G);
searchGraph.addNode(H);
searchGraph.addLink(AB);
searchGraph.addLink(AC);
searchGraph.addLink(AD);
searchGraph.addLink(BD);
searchGraph.addLink(CB);
searchGraph.addLink(CE);
searchGraph.addLink(CF);
searchGraph.addLink(DE);
searchGraph.addLink(DH);
searchGraph.addLink(FE);
searchGraph.addLink(GE);
searchGraph.addLink(HG);
// run hill-climbing search to get from start to
// destination
searchGraph.hillTraverse(
searchGraph.nodes.get(searchGraph.nodes.indexOf(A)),
searchGraph.nodes.get(searchGraph.nodes.indexOf(E)));
// display path created using hillTraverse
// This will be display the path from start to
// destination
System.out.println("the path from your current city to the destination city is: ");
for (int i = searchGraph.hillPath.size() - 1; i >= 0; i--) {
int count = 0;
if (i != 0) {
for (int j = searchGraph.hillPath.size() - 1; j >= 0; j--) {
if (searchGraph.hillPath.get(i).cityName == searchGraph.hillPath
.get(j).cityName)
count++;
}
if (count == 1)
System.out.print(searchGraph.hillPath.get(i).cityName + "->");
} else
System.out.print(searchGraph.hillPath.get(i).cityName);
}
}
static int dist(int x1, int y1, int x2, int y2) {
int distance = 0;
distance = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
return distance;
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch7p1_Link.java
//Represents a link between two GraphNodes
//******************************************************************
public class ch7p1_Link {
ch7p1_GraphNode from;
ch7p1_GraphNode to;
int distance;
// boolean skip is used for traversal to determine if the path // has already
// been visited or not
boolean skip;
public ch7p1_Link(ch7p1_GraphNode from, ch7p1_GraphNode to, int d) {
this.from = from;
this.to = to;
distance = d;
skip = false;
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3
// ch7p1_GraphNode.java
//Represents name and coordinates of a node on a graph
//******************************************************************
public class ch7p1_GraphNode {
String cityName;
int xLocation, yLocation;
public ch7p1_GraphNode(String cityName, int xLocation, int yLocation) {
this.cityName = cityName;
this.xLocation = xLocation;
this.yLocation = yLocation;
}
public String toString() {
return cityName + " ("+xLocation +"," + yLocation + ")";
}
}
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch7p1_Graph.java
//Represents GraphNodes connected through Links, including method //for doing a hill climbing
//search traversal of the graph.
//the method hillTraverse creates a hillPath list which the robot //will follow to demonstrate
//how the hill-climbing search works.
//******************************************************************************************
import java.util.ArrayList;
import java.util.Stack;
public class ch7p1_Graph {
// nodes and links define the physical creation of your Graph
ArrayList<ch7p1_GraphNode> nodes;
ArrayList<ch7p1_Link> links;
// Two lists used for traversing
ArrayList<ch7p1_GraphNode> hillTraverse;
Stack<ch7p1_Link> travelStack = new Stack<ch7p1_Link>();
// List used to define where you would like to move
ArrayList<ch7p1_GraphNode> hillPath = new ArrayList<ch7p1_GraphNode>();
// Constructor of ch7p1_Graph class
public ch7p1_Graph() {
nodes = new ArrayList<ch7p1_GraphNode>();
links = new ArrayList<ch7p1_Link>();
hillTraverse = new ArrayList<ch7p1_GraphNode>();
}// end constructor
// addNode()
// Add a city node to the graph
public void addNode(ch7p1_GraphNode node) {
nodes.add(node);
}// end addNode
// addLink
// Add link to the graph
public void addLink(ch7p1_Link link) {
links.add(link);
}// end addLink
// hillTraverse()
// perform hill-climbing search on the graph
public void hillTraverse(ch7p1_GraphNode from, ch7p1_GraphNode to) {
// boolean matched;
int distance;
ch7p1_Link found;
// determine if there is a link between from and to
// if there is a match then add the link to the
// travelStack and
// add the nodes to hillPath
// This will ultimately repeated by the end of the
// search
distance = match(from, to);
if (distance != 0) {
travelStack.push(new ch7p1_Link(from, to, distance));
hillPath.add(new ch7p1_GraphNode(to.cityName, to.xLocation,
to.yLocation));
hillPath.add(new ch7p1_GraphNode(from.cityName, from.xLocation,
from.yLocation));
return;
}
// if there is no match found you could another path
// findings
found = find(from);
// if you find a new connection then you could add it // to the travelStack
// and
// and the start node to hillPath
// recursively call hillTraverse with the link's to as // start and our
// destination as the end
if (found != null) {
travelStack.push(new ch7p1_Link(from, to, found.distance));
hillTraverse(found.to, to);
hillPath.add(new ch7p1_GraphNode(from.cityName, from.xLocation,
from.yLocation));
}
// backtrack if you cannot find a new connection
else if (travelStack.size() > 0) {
found = travelStack.pop();
hillTraverse(found.from, found.to);
hillPath.add(new ch7p1_GraphNode(from.cityName, from.xLocation,
from.yLocation));
}
}// end hillTraverse()
// find() method is used to
// find the next link to try exploring
public ch7p1_Link find(ch7p1_GraphNode from) {
int pos = -1;
int dist = 0;
// iterate through the list of links
for (int a = 0; a < links.size(); a++) {
if (links.get(a).from.equals(from) && !links.get(a).skip) {
// Use the longest flight.
if (links.get(a).distance > dist) {
pos = a;
dist = links.get(a).distance;
}
}
}
// link found
if (pos != -1) {
// mark this link as used so we don't match it again
links.get(pos).skip = true;
ch7p1_Link foundList = new ch7p1_Link(links.get(pos).from,
links.get(pos).to, links.get(pos).distance);
return foundList;
}
return null; // not found
}// end find()
// match() method is used to determine if there is a link
// between a starting
// node and an ending node
public int match(ch7p1_GraphNode from, ch7p1_GraphNode to) {
// iterate through list of links
for (int a = links.size() - 1; a >= 0; a--) {
if (links.get(a).from.equals(from) && links.get(a).to.equals(to)
&& !links.get(a).skip) {
links.get(a).skip = true;
return links.get(a).distance;
}
}
return 0;
}// end match()
}// end Graph.java给定startCityA 和endCityE，运行程序的结果如下:
the path from your current city to the destination city is: A->C->E
此结果显示了使用爬山搜索策略遍历图形的准确路径，不包括中间回溯节点。
基于莱霍斯·EV3 的爬山算法
在这一节中，你需要为你的机器人开发一个程序，使它能够在起始节点 A 和目的节点 E 之间的路径上行进，如图 7-3 所示，使用最少的节点数，并使用爬山搜索算法。
您的程序应至少在 LCD 上显示目的地的坐标，然后显示一条消息“Press ENTER key to continue”当回车键被按下时，你的机器人应该移动到下一个节点。例如，假设你的机器人从A (0,0)出发，想要探索一条通往E (0,30)的道路。假设你的机器人使用爬山搜索通过最少节点的路径是A -> C -> E。在起点 A，您的程序应该执行以下操作:
- 在 LCD 上显示目的地的坐标C(-15, 20)。
- 显示信息“Press ENTER key to continue”。
- 去坐标为-15，20 的地方。
- 在 LCD 上显示目的地的坐标E(0, 30)。
- 显示信息“Press ENTER key to continue.”
- 转到坐标为 0，30 的位置。
而且，你的问题应该有一个名为 destination 的字符串，这样它就足够智能，当改变 destination 的值时，你的机器人可以探索一条从起始节点 A 到新的目的节点的新路径，机器人将通过最少数量的节点到达目的节点。(我们假设起始节点总是 A，所以 from 字符串可以硬编码。)
以下程序表示基于 leJOS 的爬山算法的实现，该算法被设计成探索从节点 A 到目的节点 E 的路径:
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch7p2_main.java
//Driver class to set up map using ch7p2_GraphNode, ch7p2_Link, and //ch7p2_Graph classes.
//Calls a hill-climbing search in ch7p2_Graph class to create //navigation path from a start
//and end node and then robots will follow the path to move from //start node
//to destination node
//******************************************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.MovePilot;
import lejos.robotics.navigation.Navigator;
public class ch7p2_main {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.C);
public static void main(String[] args) {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
// setup the wheel diameter of left (and right) motor // in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot ev3robot = new MovePilot(chassis);
Navigator navbot = new Navigator(ev3robot);
// These objects used to define what your graph looks // like
ch7p2_Graph searchGraph = new ch7p2_Graph();
ch7p2_GraphNode A, B, C, D, E, F, G, H;
ch7p2_Link AB, AC, AD, BD, CB, CE, CF, DE, DH, FE, GE, HG;
// define each node
A = new ch7p2_GraphNode("A", 0, 0);
B = new ch7p2_GraphNode("B", -10, 20);
C = new ch7p2_GraphNode("C", -15, 20);
D = new ch7p2_GraphNode("D", 0, 20);
E = new ch7p2_GraphNode("E", 0, 30);
F = new ch7p2_GraphNode("F", -15, 30);
G = new ch7p2_GraphNode("G", 10, 20);
H = new ch7p2_GraphNode("H", 10, 10);
// define which GraphNodes are connected
AB = new ch7p2_Link(A, B, dist(A.xLocation, A.yLocation, B.xLocation,
B.yLocation));
AC = new ch7p2_Link(A, C, dist(A.xLocation, A.yLocation, C.xLocation,
C.yLocation));
AD = new ch7p2_Link(A, D, dist(A.xLocation, A.yLocation, D.xLocation,
D.yLocation));
BD = new ch7p2_Link(B, D, dist(B.xLocation, B.yLocation, D.xLocation,
D.yLocation));
CB = new ch7p2_Link(C, B, dist(C.xLocation, C.yLocation, B.xLocation,
B.yLocation));
CE = new ch7p2_Link(C, E, dist(C.xLocation, C.yLocation, E.xLocation,
E.yLocation));
CF = new ch7p2_Link(C, F, dist(C.xLocation, C.yLocation, F.xLocation,
F.yLocation));
DE = new ch7p2_Link(D, E, dist(D.xLocation, D.yLocation, E.xLocation,
E.yLocation));
DH = new ch7p2_Link(D, H, dist(D.xLocation, D.yLocation, H.xLocation,
H.yLocation));
FE = new ch7p2_Link(F, E, dist(F.xLocation, F.yLocation, E.xLocation,
E.yLocation));
GE = new ch7p2_Link(G, E, dist(G.xLocation, G.yLocation, E.xLocation,
E.yLocation));
HG = new ch7p2_Link(H, G, dist(H.xLocation, H.yLocation, G.xLocation,
G.yLocation));
// add all nodes and links to your graph object
searchGraph.addNode(A);
searchGraph.addNode(B);
searchGraph.addNode(C);
searchGraph.addNode(D);
searchGraph.addNode(E);
searchGraph.addNode(F);
searchGraph.addNode(G);
searchGraph.addNode(H);
searchGraph.addLink(AB);
searchGraph.addLink(AC);
searchGraph.addLink(AD);
searchGraph.addLink(BD);
searchGraph.addLink(CB);
searchGraph.addLink(CE);
searchGraph.addLink(CF);
searchGraph.addLink(DE);
searchGraph.addLink(DH);
searchGraph.addLink(FE);
searchGraph.addLink(GE);
searchGraph.addLink(HG);
// run hill-climbing search to get from start to
// destination
searchGraph.hillTraverse(
searchGraph.nodes.get(searchGraph.nodes.indexOf(A)),
searchGraph.nodes.get(searchGraph.nodes.indexOf(E)));
// block the thread until a button is pressed
buttons.waitForAnyPress();
// Robot moves through path from start to destination // by using
// hillTraverse
for (int i = searchGraph.hillPath.size() - 1; i >= 0; i--) {
int count = 0;
for (int j = searchGraph.hillPath.size() - 1; j >= 0; j--) {
if (searchGraph.hillPath.get(i).cityName == searchGraph.hillPath
.get(j).cityName)
count++;
}
if (count == 1) {
// go to node
navbot.goTo(searchGraph.hillPath.get(i).xLocation,
searchGraph.hillPath.get(i).yLocation);
LCD.clear();
// display current location
LCD.drawString("At location "
+ searchGraph.hillPath.get(i).cityName + ", ", 0, 0);
LCD.drawString(searchGraph.hillPath.get(i).xLocation + ", "
+ searchGraph.hillPath.get(i).yLocation, 0, 1);
LCD.drawString("Press ENTER key", 0, 2);
// block the thread until a button is pressed
buttons.waitForAnyPress();
}
}
}
static int dist(int x1, int y1, int x2, int y2) {
int distance = 0;
distance = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
return distance;
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch7p2_Link.java
//Represents a link between two GraphNodes);
//******************************************************************
public class ch7p2_Link {
ch7p2_GraphNode from;
ch7p2_GraphNode to;
int distance;
// boolean skip is used for traversal to determine if the path // has already
// been visited or not
boolean skip;
public ch7p2_Link(ch7p2_GraphNode from, ch7p2_GraphNode to, int d) {
this.from = from;
this.to = to;
distance = d;
skip = false;
}
}
//************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 //ch7p1_GraphNode.java
//Represents name and coordinates of a node on a graph
//************************************************************************
public class ch7p2_GraphNode {
String cityName;
int xLocation, yLocation;
public ch7p2_GraphNode(String cityName, int xLocation, int yLocation) {
this.cityName = cityName;
this.xLocation = xLocation;
this.yLocation = yLocation;
}
public String toString() {
return cityName + " (" + xLocation + "," + yLocation + ")";
}
}
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch7p2_Graph.java
//Represents GraphNodes connected through Links, including method //for doing a hill climbing
//search traversal of the graph.
//the method hillTraverse creates a hillPath list which the robot //will follow to demonstrate
//how the hill-climbing search works.
//******************************************************************************************
import java.util.ArrayList;
import java.util.Stack;
public class ch7p2_Graph {
// nodes and links define the physical creation of your Graph
ArrayList<ch7p2_GraphNode> nodes;
ArrayList<ch7p2_Link> links;
// Two lists used for traversing
ArrayList<ch7p2_GraphNode> hillTraverse;
Stack<ch7p2_Link> travelStack = new Stack<ch7p2_Link>();
// List used to define where you would like to move
ArrayList<ch7p2_GraphNode> hillPath = new ArrayList<ch7p2_GraphNode>();
// Constructor of ch7p2_Graph class
public ch7p2_Graph() {
nodes = new ArrayList<ch7p2_GraphNode>();
links = new ArrayList<ch7p2_Link>();
hillTraverse = new ArrayList<ch7p2_GraphNode>();
}// end constructor
// addNode()
// Add a city node to the graph
public void addNode(ch7p2_GraphNode node) {
nodes.add(node);
}// end addNode
// addLink
// Add link to the graph
public void addLink(ch7p2_Link link) {
links.add(link);
}// end addLink
// hillTraverse()
// perform hill-climbing search on the graph
public void hillTraverse(ch7p2_GraphNode from, ch7p2_GraphNode to) {
// boolean matched;
int distance;
ch7p2_Link found;
// determine if there is a link between from and to
// if there is a match then add the link to the
// travelStack and
// add the nodes to hillPath
// This will ultimately repeated by the end of the
// search
distance = match(from, to);
if (distance != 0) {
travelStack.push(new ch7p2_Link(from, to, distance));
hillPath.add(new ch7p2_GraphNode(to.cityName, to.xLocation,
to.yLocation));
hillPath.add(new ch7p2_GraphNode(from.cityName, from.xLocation,
from.yLocation));
return;
}
// if there is no match found you could another path
// findings
found = find(from);
// if you find a new connection then you could add it // to the travelStack
// and
// and the start node to hillPath
// recursively call hillTraverse with the link's to as // start and our
// destination as the end
if (found != null) {
travelStack.push(new ch7p2_Link(from, to, found.distance));
hillTraverse(found.to, to);
hillPath.add(new ch7p2_GraphNode(from.cityName, from.xLocation,
from.yLocation));
}
// backtrack if you cannot find a new connection
else if (travelStack.size() > 0) {
found = travelStack.pop();
hillTraverse(found.from, found.to);
hillPath.add(new ch7p2_GraphNode(from.cityName, from.xLocation,
from.yLocation));
}
}// end hillTraverse()
// find() method is used to
// find the next link to try exploring);
public ch7p2_Link find(ch7p2_GraphNode from) {
int pos = -1;
int dist = 0;
// iterate through the list of links
for (int a = 0; a < links.size(); a++) {
if (links.get(a).from.equals(from) && !links.get(a).skip) {
// Use the longest flight.
if (links.get(a).distance > dist) {
pos = a;
dist = links.get(a).distance;
}
}
}
// link found
if (pos != -1) {
// mark this link as used so we don't match it
// again
links.get(pos).skip = true;
ch7p2_Link foundList = new ch7p2_Link(links.get(pos).from,
links.get(pos).to, links.get(pos).distance);
return foundList;
}
return null; // not found
}// end find()
// match() method is used to determine if there is a link
// between a starting
// node and an ending node
public int match(ch7p2_GraphNode from, ch7p2_GraphNode to) {
// iterate through list of links
for (int a = links.size() - 1; a >= 0; a--) {
if (links.get(a).from.equals(from) && links.get(a).to.equals(to)
&& !links.get(a).skip) {
links.get(a).skip = true;
return links.get(a).distance;
}
}
return 0;
}// end match()
}// end Graph.java);摘要
在这一章中，你学习了启发式搜索和爬山算法的基础。你现在知道如何在实践中应用爬山搜索算法来解决一个搜索程序。您还学习了如何基于爬山搜索算法和导航类来构建解决问题的代理，在前面的章节中，这些代理使用最短的连接数智能地找到从起点到任何目的地的路线。
在下一章中，您将学习 Dijkstra 算法，该算法可用于寻找最优解，而不是爬山中的启发式函数所定义的(或“任何”)解。此外，您还将学习如何将 Dijkstra 的算法集成到基于 leJOS 的机器人系统中，以进行定位和路径规划。
八、Dijkstra 算法及其在 Lego Mindstorms 上的实现
继第七章对启发式搜索算法的讨论之后，本章将讨论 Dijkstra 算法。该算法可用于寻找最优解，而不是由爬山算法中的启发式函数定义的“或任何”解。特别是，本章将涵盖以下主题:
- 一个新的 Dijkstra 的搜索算法，可以应用于建立任意树结构一般。
- 在基于 leJOS 的机器人系统中应用和集成所提出的 Dijkstra 算法用于定位和路径规划，这增强了 leJOS 系统中现有的寻路方法。
Dijkstra 算法简介
在第七章中讨论的爬山算法试图最小化连接的数量。如你所知，这样的爬山搜索找到了一条好的路线——不是最好的，但是可以接受的。此外，迄今为止介绍的所有先前的搜索算法(包括 BSF 和 DFS)都能够确定一个解决方案:即，任何解决方案，不一定是最佳解决方案或者甚至是“好的”解决方案。定义试探法有助于提高找到好的解决方案的可能性；然而，没有作出任何努力来确保找到最佳解决方案。为了获得最优解，即最佳解，需要应用众所周知的 Dijkstra 算法:
在任何给定的图中和在任何起始节点，Dijkstra 的算法发现从起始节点到所有其他节点的最短路径。
图 8-1 显示了表示连接节点的图形:节点是标有 A-J 的蓝色圆圈。路径是连接两个节点的蓝色线，每条路径旁边都有一个关联的距离。注意:长度不是按比例的。
图 8-1。
Graphic representation of connected nodes
节点 A 是您的起始节点，您希望找到到达图中所有其他节点的最短路径。为此，您需要生成一个表。此表包含从起始节点 a 的角度到图形中所有节点的距离。
如表 8-1 所示，距离的初始条目都被设置为无穷大(或某个名义上的最大值)。这确保了找到的任何路径都比存储在表中的初始值短。
表 8-1。
Initial entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 || --- | --- |
| B | 无限的 |
| C | 无限的 |
| D | 无限的 |
| E | 无限的 |
| F | 无限的 |
| G | 无限的 |
| H | 无限的 |
| 我 | 无限的 |
| J | 无限的 |
节点 A 是起始节点，因此您将首先检查离开该节点的所有可能路径。选项如表 8-2 所示。
表 8-2。
Distance to nodes (from Node A) accessible from Node A
| 结节 | 从节点 A 到节点的距离 || --- | --- |
| B | seven |
| C | Two |
| D | three |
| E | Seventeen |
这些值用于更新表 8-1 ，因此您现在有了表 8-3 。
表 8-3。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 || --- | --- |
| B | seven |
| C | Two |
| D | three |
| E | Seventeen |
| F | 无限的 |
| G | 无限的 |
| H | 无限的 |
| 我 | 无限的 |
| J | 无限的 |
图 8-2 显示了红色标记的路线。从节点 a 有四条路径。但是，这些路径还不能保证是最短路径。为了确定你已经确定了最短的路径，你必须继续前进。
图 8-2。
Graphic representation in which starting node A has been visited
算法中的下一步是前往距离节点 a 最近的节点。在这种情况下，即节点 C。在节点 C，您有到节点 B、G 和 j 的可用路径。计算距离时，您必须确定它们与节点 a 的距离。新距离如表 8-4 所示。
表 8-4。
Distance to nodes accessible from Node A
| 结节 | 从节点 A 到节点的距离 || --- | --- |
| B | five |
| G | Thirty-one |
| J | nine |
然后将这些值与表 8-3 中的值进行比较。您会发现这两个值都小于表中存储的当前值。这样，工作台 8-3 转换为工作台 8-5 如下。
表 8-5。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 || --- | --- |
| B | five |
| C | Two |
| D | three |
| E | Seventeen |
| F | 无限的 |
| G | Thirty-one |
| H | 无限的 |
| 我 | 无限的 |
| J | nine |
这一步说明了 Dijkstra 算法的一个优点:到 Node B 的路由不是最直接的路由，但却是最短的。Dijkstra 的算法可以找到最短的路线，即使这条路线不是最直接的。
同样，已经检查了从节点 C 可访问的所有路径，并且已经更新了路径表。节点 C 被标记为已被访问，如图 8-3 所示。
图 8-3。
Graphic representation in which Node C has been visited
在 Dijkstra 的算法中，被访问的节点永远不会被重新访问。此外，一旦节点被标记为已访问，到该节点的路径就是从初始节点开始的最短路径。
因此，您应该向您的表中添加另一列，如表 8-6 所示。
表 8-6。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | 访问 || --- | --- | --- |
| B | five | 不 |
| C | Two | 是 |
| D | three | 不 |
| E | Seventeen | 不 |
| F | 无限的 | 不 |
| G | Thirty-one | 不 |
| H | 无限的 | 不 |
| 我 | 无限的 | 不 |
| J | nine | 不 |
当这些值被更新时，伴随这些距离的路线也需要被存储。再次检查路径表，并找到到未被访问的节点的最短路径。该节点成为下一个当前节点。在这种情况下，它是节点 D。从节点 D 可以使用以下路径:
表 8-7。
Distance to nodes accessible from Node A
| 结节 | 从节点 A 到节点的距离 || --- | --- |
| J | Eleven |
| 我 | Sixteen |
| E | eight |
所有路径的表被更新以反映这一点，并且节点 D 被标记为已访问。这也锁定了到节点 D 的最短路径。
从表 8-8 中可以看出，距离节点 A 最近的节点是节点 B。接下来检查来自节点 B 的所有路径。在本例中，您有一条到标记为已访问的节点(节点 C)的路径。我们已经知道，到节点 C 的路径是最短的。
表 8-8。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | 访问 || --- | --- | --- |
| B | five | 不 |
| C | Two | 是 |
| D | three | 是 |
| E | eight | 不 |
| F | 无限的 | 不 |
| G | Thirty-one | 不 |
| H | 无限的 | 不 |
| 我 | Sixteen | 不 |
| J | nine | 不 |
如图 8-4 所示，当您检查路径时，从节点 B 可访问的唯一其他节点是节点 f。这将更新我们的路径，如表 8-9 所示:
表 8-9。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | 访问 || --- | --- | --- |
| B | five | 是 |
| C | Two | 是 |
| D | three | 是 |
| E | eight | 不 |
| F | nine | 不 |
| G | Thirty-one | 不 |
| H | 无限的 | 不 |
| 我 | Sixteen | 不 |
| J | nine | 不 |
图 8-4。
Graphic representation with Node C and Node D marked visited
表 8-9 再次告诉我们，您要访问的下一个节点是节点 e。然后，您将路径相加，并将这些节点标记为已访问。接下来检查路径，从节点 E 可访问的唯一其他节点是节点 I。从节点 A 到节点 I 的距离是 27，大于表中所示的距离。因此，在这一轮中没有对表的更新，节点 E 被标记为已访问。继续看表 8-9 ，你知道下一个要访问的节点是节点 F。如图 8-4 所示，当你检查路径时，从节点 F 唯一可访问的另一个节点是节点 g。这更新了如表 8-10 所示的路径。
表 8-10。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | 访问 || --- | --- | --- |
| B | five | 是 |
| C | Two | 是 |
| D | three | 是 |
| E | eight | 是 |
| F | nine | 是 |
| G | Nineteen | 不 |
| H | 无限的 | 不 |
| 我 | Sixteen | 不 |
| J | nine | 不 |
表 8-10 告诉我们你要访问的下一个节点是节点 j。然后你把路径加起来，把节点标记为已访问。接下来检查路径，从节点 J 可访问的唯一其他节点是节点 h。这将更新路径，如表 8-11 所示。
表 8-11。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | 访问 || --- | --- | --- |
| B | five | 是 |
| C | Two | 是 |
| D | three | 是 |
| E | eight | 是 |
| F | nine | 是 |
| G | Nineteen | 不 |
| H | Twenty-seven | 不 |
| 我 | Sixteen | 不 |
| J | nine | 是 |
表 8-11 告诉我们您要访问的下一个节点是节点 I。接下来，您将路径相加，并将节点标记为已访问。当您检查路径时，从节点 I 可访问的唯一其他节点是节点 h。这将更新路径，如表 8-12 所示。
表 8-12。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | 访问 || --- | --- | --- |
| B | five | 是 |
| C | Two | 是 |
| D | three | 是 |
| E | eight | 是 |
| F | nine | 是 |
| G | Nineteen | 不 |
| H | Twenty-two | 不 |
| 我 | Sixteen | 是 |
| J | nine | 是 |
表 8-12 告诉我们下一个要访问的节点是节点 g。然后你把路径加起来，把节点标记为已访问。检查路径时，从节点 G 可访问的唯一其他节点是节点 H。从节点 A 到节点 H 的距离是 36，大于表中所示的 22。因此，表没有更新，节点 G 被标记为已访问。现在已经访问了所有节点，搜索将终止。最终，您将看到表 8-13 ，该表显示了图 8-1 所示图形中从节点 A 到所有其他节点的最短距离。
表 8-13。
Entries for distances to nodes from Node A
| 点头 | 从节点 A 到节点的距离 | 访问 || --- | --- | --- |
| B | five | 是 |
| C | Two | 是 |
| D | three | 是 |
| E | eight | 是 |
| F | nine | 是 |
| G | Nineteen | 是 |
| H | Twenty-two | 是 |
| 我 | Sixteen | 是 |
| J | nine | 是 |
使用图 8-1 所示的图形，下面的程序使用 Dijkstra 算法寻找从起始节点 A 到所有其他节点的最短路径。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3ch8p1_main.java
//Driver class to set up map using ch8p1_edge, find the shortest //path from
//starting node A to all the other nodes in a given graph.
//******************************************************************
import java.util.ArrayList;
public class ch8p1_main {
static ArrayList<ch8p1_edge> graph = null;
static ch8p1_edge[] parents = null;
static ArrayList<String> unsolvedConn = null;
static ArrayList<String> solvedConn = null;
private static ch8p1_minpath finalPath;
public static void main(String[] args) {
// initialize the nodes set
String[] nodes = { "A", "B", "C", "D", "E", "F", "G", "H", "I", "J" };
// initialize the map with the nodes
graph = new ArrayList<ch8p1_edge>();
graph.add(new ch8p1_edge("A", "B", 7));
graph.add(new ch8p1_edge("A", "C", 2));
graph.add(new ch8p1_edge("A", "D", 3));
graph.add(new ch8p1_edge("A", "E", 17));
graph.add(new ch8p1_edge("B", "F", 4));
graph.add(new ch8p1_edge("C", "B", 3));
graph.add(new ch8p1_edge("C", "G", 29));
graph.add(new ch8p1_edge("C", "J", 7));
graph.add(new ch8p1_edge("D", "J", 8));
graph.add(new ch8p1_edge("D", "I", 13));
graph.add(new ch8p1_edge("D", "E", 5));
graph.add(new ch8p1_edge("E", "I", 19));
graph.add(new ch8p1_edge("F", "G", 10));
graph.add(new ch8p1_edge("G", "H", 17));
graph.add(new ch8p1_edge("J", "H", 18));
graph.add(new ch8p1_edge("I", "H", 6));
// initialize the unsolved nodes
unsolvedConn = new ArrayList<String>();
// sets the parent node in the unsolved connection
// ArrayList to A
unsolvedConn.add(nodes[0]);
// initialize the solved nodes
solvedConn = new ArrayList<String>();
// Add all nodes to the solved connection tree
for (int i = 1; i < nodes.length; i++)
solvedConn.add(nodes[i]);
// create a parent array that will store all the edges
parents = new ch8p1_edge[nodes.length];
// Set the initial node to A and make its parent null // with a weight cost
// of zero
parents[0] = new ch8p1_edge(null, nodes[0], 0);
for (int i = 0; i < solvedConn.size(); i++) {
// get all of the String node names that could be // attached the
// root
String n = solvedConn.get(i);
// Check the weights of all the nodes that are
// attached to the root
// A node
// If they are attached will return positive
// weight if not will
// return -1
parents[i + 1] = new ch8p1_edge(nodes[0], n, getEdgeLength(
nodes[0], n));
}
finalPath = null;
// while the solved nodes ArrayList is greater than zero
while (solvedConn.size() > 0) {
// Create a minimum shortest path object to find // the shortest path
// to all connected points
ch8p1_minpath msp = getMinSideNode();
finalPath = msp;
if (msp.getEdgeLength() == -1)
msp.outputPath(nodes[0]);
else
msp.outputPath();
String node = msp.getLastNode();
unsolvedConn.add(node);
setEdgeLength(node);
}
}
public static String getParent(ch8p1_edge[] parents, String node) {
if (parents != null) {
for (ch8p1_edge nd : parents) {
if (nd.getChildNode() == node) {
return nd.getParentNode();
}
}
}
return null;
}
public static void setEdgeLength(String parentNode) {
if (graph != null && parents != null && solvedConn != null) {
for (String node : solvedConn) {
ch8p1_minpath msp = getMinPath(node);
int w1 = msp.getEdgeLength();
if (w1 == -1)
continue;
for (ch8p1_edge n : parents) {
if (n.getChildNode() == node) {
if (n.getEdgeLength() == -1 || n.getEdgeLength() > w1) {
n.setEdgeLength(w1);
n.setParentNode(parentNode);
break;
}
}
}
}
}
}
public static int getEdgeLength(String parentNode, String childNode) {
if (graph != null) {
for (ch8p1_edge s : graph) {
if (s.getParentNode() == parentNode
&& s.getChildNode() == childNode)
return s.getEdgeLength();
}
}
return -1;
}
public static ch8p1_minpath getMinSideNode() {
// Create a minimum shortest path object
ch8p1_minpath minMsp = null;
// while the solved node ArrayList is greater than zero
if (solvedConn.size() > 0) {
// Create an index value set to zero
int index = 0;
// for each value in the solved nodes ArrayList
for (int j = 0; j < solvedConn.size(); j++) {
// Create a shortest path map get the
// MinPath of the solved node
ch8p1_minpath msp = getMinPath(solvedConn.get(j));
// if there is no minimum shortest path, // if the minimum shortest
// path
// does not equal -1
// AND the minimum shortest path get
// weight is less than the
// minimum shortest path get weight
if (minMsp == null || msp.getEdgeLength() != -1
&& msp.getEdgeLength() < minMsp.getEdgeLength()) {
// set the minimum shortest path to // the minimum shortest
// path
minMsp = msp;
// set the index value equal it the // node value j
index = j;
}
}
// remove the index that you checked in the
// solved nodes
solvedConn.remove(index);
}
// return the MinShortPath object
return minMsp;
}
public static ch8p1_minpath getMinPath(String node) {
// Create a Minshort Path object that is an ArrayList // and set the take
// in node as the base
// in this case will always be zero
ch8p1_minpath msp = new ch8p1_minpath(node);
// if the parents array does not equal null and the
// unsolved nodes does
// not equal null
if (parents != null && unsolvedConn != null) {
for (int i = 0; i < unsolvedConn.size(); i++) {
ch8p1_minpath tempMsp = new ch8p1_minpath(node);
String parent = unsolvedConn.get(i);
String curNode = node;
while (parent != null) {
int weight = getEdgeLength(parent, curNode);
if (weight > -1) {
tempMsp.addNode(parent);
tempMsp.addEdgeLength(weight);
curNode = parent;
parent = getParent(parents, parent);
} else
break;
}
if (msp.getEdgeLength() == -1 || tempMsp.getEdgeLength() != -1
&& msp.getEdgeLength() > tempMsp.getEdgeLength())
msp = tempMsp;
}
}
return msp;
}
}
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch8p1_minpath.java
//the minimum path from starting node to all the other nodes, //including each node
//and the corresponding minimum distance as well
//******************************************************************************************
import java.util.ArrayList;
public class ch8p1_minpath {
private ArrayList<String> nodeList;
private int edgeLength;
public ch8p1_minpath(String node) {
nodeList = new ArrayList<String>();
nodeList.add(node);
edgeLength = -1;
}
public ArrayList<String> getNodeList() {
return nodeList;
}
public void setNodeList(ArrayList<String> nodeList) {
this.nodeList = nodeList;
}
public void addNode(String node) {
if (nodeList == null)
nodeList = new ArrayList<String>();
nodeList.add(0, node);
}
public String getLastNode() {
int size = nodeList.size();
return nodeList.get(size - 1);
}
public int getEdgeLength() {
return edgeLength;
}
public void setEdgeLength(int edgeLength) {
this.edgeLength = edgeLength;
}
public void outputPath() {
outputPath(null);
}
public void outputPath(String pathNode) {
String result = "the minimum path of ";
if (pathNode != null)
nodeList.add(pathNode);
for (int i = 0; i < nodeList.size(); i++) {
result += "" + nodeList.get(i);
if (i < nodeList.size() - 1)
result += "->";
}
result += " is:" + edgeLength;
System.out.println(result);
}
public void addEdgeLength(int e) {
if (edgeLength == -1)
edgeLength = e;
else
edgeLength += e;
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch8p1_edge.java
//edge class including the parent node and child node
//and the edge length between two nodes
//******************************************************************
public class ch8p1_edge {
private String parentNode;
private String childNode;
private int edgeLength;
public ch8p1_edge(String parentNode, String childNode, int edgeLength) {
this.parentNode = parentNode;
this.childNode = childNode;
this.edgeLength = edgeLength;
}
public String getParentNode() {
return parentNode;
}
public void setParentNode(String parentNode) {
this.parentNode = parentNode;
}
public String getChildNode() {
return childNode;
}
public void setChildNode(String childNode) {
this.childNode = childNode;
}
public int getEdgeLength() {
return edgeLength;
}
public void setEdgeLength(int edgeLength) {
this.edgeLength = edgeLength;
}
}运行程序的结果如下:
The minimum path of A->C is: 2
The minimum path of A->D is: 3
The minimum path of A->C->B is: 5
The minimum path of A->D->E is: 8
The minimum path of A->C->B->F is: 9
The minimum path of A->C->J is: 9
The minimum path of A->D->I is:16
The minimum path of A->C->B->F->G is: 19
The minimum path of A->D->I->H is: 22如您所见，这个结果与表 8-13 中显示的结果完全相同。
基于莱霍斯·EV3 的迪杰斯特拉算法
参考图 8-5 ，你将为你的机器人编写一个程序，使它能够使用 Dijkstra 算法在起始节点 A 和目的节点 I 之间行进尽可能短的距离。假设您使用笛卡尔坐标系:
- A 处的坐标是(0，0)。
- B 点的坐标是(-10，20)。
- C 点的坐标是(0，20)。
- D 点的坐标是(-15，20)。
- E 点的坐标是(-15，30)。
- F 点的坐标是(0，30)。
- 在 G 点的坐标是(10，10)。
- H 点的坐标是(10，20)。
- I 处的坐标是(10，30)。
图 8-5。
Graphic to be used for conducting the shortest-path search
您的程序应至少在 LCD 上显示目的地的坐标，然后显示消息“Press ENTER Key to continue”当你按下回车键，你的机器人移动到下一个节点。例如，假设你的机器人从Node A(0,0)出发，你想让它探索一条通往Node G(10,10)的道路。假设您的机器人使用 Dijkstra 算法探索的最小距离路径是Node A -> Node C -> Node G。在起点节点 A，您的程序应该执行以下操作:
- 在 LCD 上显示目的地的坐标Node C(0, 20)。
- 显示信息“Press ENTER key to continue.”
- 转到坐标为 0，20 的位置。
- 在 LCD 上显示目的地的坐标Node G(10, 10)。
- 显示信息“Press ENTER key to continue.”
- 前往坐标为 10，10 的位置。
此外，您的问题应该有一个名为 destination 的字符串，这样当更改 destination 的值时，它就足够智能，您的机器人可以探索一条从起始节点 A 到新的目的节点的新路径，在这条路径上，机器人将以最小的距离到达目的节点。(我们假设起始节点总是 A，所以 from 字符串可以硬编码。)
以下程序表示基于 leJOS 的 Dijkstra 算法的实现，该算法被设计成探索从节点 A 到目的节点 I 的最短路径:
//******************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch8p2_main.java
//Driver class to set up map using ch8p2_edge, find the shortest //path from
//starting node A to all the other nodes in a given graph.
//and then robots will follow the path to move from start node A
//to the given destination node I
//******************************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.MovePilot;
import lejos.robotics.navigation.Navigator;
import lejos.robotics.navigation.Waypoint;
import lejos.robotics.pathfinding.Path;
import java.util.ArrayList;
public class ch8p2_main {
static ArrayList<ch8p2_edge> graph = null;
static ch8p2_edge[] parents = null;
static ArrayList<String> unsolvedConn = null;
static ArrayList<String> solvedConn = null;
private static ch8p2_minpath finalPath;
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.C);
static Waypoint[] coordinates = { new Waypoint(0, 0),
new Waypoint(-10, 20), new Waypoint(0, 20), new Waypoint(-15, 20),
new Waypoint(-15, 30), new Waypoint(0, 30), new Waypoint(10, 10),
new Waypoint(10, 20), new Waypoint(10, 30),
};
public static void main(String[] args) {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
// setup the wheel diameter of left (and right) motor // in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot ev3robot = new MovePilot(chassis);
Navigator navbot = new Navigator(ev3robot);
// initialize the nodes set
String[] nodes = { "A", "B", "C", "D", "E", "F", "G", "H", "I" };
// initialize the map with the nodes
graph = new ArrayList<ch8p2_edge>();
graph.add(new ch8p2_edge("A", "B", 22));
graph.add(new ch8p2_edge("A", "C", 20));
graph.add(new ch8p2_edge("A", "D", 25));
graph.add(new ch8p2_edge("B", "C", 10));
graph.add(new ch8p2_edge("C", "E", 18));
graph.add(new ch8p2_edge("C", "F", 10));
graph.add(new ch8p2_edge("C", "G", 14));
graph.add(new ch8p2_edge("C", "H", 10));
graph.add(new ch8p2_edge("C", "I", 18));
graph.add(new ch8p2_edge("D", "B", 5));
graph.add(new ch8p2_edge("D", "E", 10));
graph.add(new ch8p2_edge("D", "F", 18));
graph.add(new ch8p2_edge("E", "F", 15));
graph.add(new ch8p2_edge("F", "I", 10));
graph.add(new ch8p2_edge("G", "H", 10));
graph.add(new ch8p2_edge("H", "F", 14));
graph.add(new ch8p2_edge("H", "I", 10));
// initialize the unsolved nodes
unsolvedConn = new ArrayList<String>();
// sets the parent node in the unsolved connection
// ArrayList to A
unsolvedConn.add(nodes[0]);
// initialize the solved nodes
solvedConn = new ArrayList<String>();
// Add all nodes to the solved connection tree
for (int i = 1; i < nodes.length; i++)
solvedConn.add(nodes[i]);
// create a parent array that will store all the edges
parents = new ch8p2_edge[nodes.length];
// Set the initial node to A and make its parent null // with a weight cost
// of zero
parents[0] = new ch8p2_edge(null, nodes[0], 0);
for (int i = 0; i < solvedConn.size(); i++) {
// get all of the String node names that could be // attached the
// root
String n = solvedConn.get(i);
// Check the weights of all the nodes that are
// attached to the root
// A node
// If they are attached will return positive
// weight if not will
// return -1
parents[i + 1] = new ch8p2_edge(nodes[0], n, getEdgeLength(
nodes[0], n));
}
finalPath = null;
// while the solved nodes ArrayList is greater than zero
while (solvedConn.size() > 0) {
// Create a minimum shortest path object to find // the shortest path
// to all connected points
ch8p2_minpath msp = getMinSideNode();
finalPath = msp;
String node = msp.getLastNode();
unsolvedConn.add(node);
setEdgeLength(node);
}
Path directions = finalPath.buildPath(coordinates);
navbot.setPath(directions);
navbot.singleStep(true);
while (navbot.getWaypoint() != null) {
if (navbot.getWaypoint() != null) {
System.out.println("Next destination" + navbot.getWaypoint());
}
System.out.println("Press Enter Key to Continue");
// block the thread until a button is pressed
buttons.waitForAnyPress();
navbot.followPath();
while (navbot.isMoving())
;
}
// block the thread until a button is pressed
buttons.waitForAnyPress();
}
public static String getParent(ch8p2_edge[] parents, String node) {
if (parents != null) {
for (ch8p2_edge nd : parents) {
if (nd.getChildNode() == node) {
return nd.getParentNode();
}
}
}
return null;
}
public static void setEdgeLength(String parentNode) {
if (graph != null && parents != null && solvedConn != null) {
for (String node : solvedConn) {
ch8p2_minpath msp = getMinPath(node);
int w1 = msp.getEdgeLength();
if (w1 == -1)
continue;
for (ch8p2_edge n : parents) {
if (n.getChildNode() == node) {
if (n.getEdgeLength() == -1 || n.getEdgeLength() > w1) {
n.setEdgeLength(w1);
n.setParentNode(parentNode);
break;
}
}
}
}
}
}
public static int getEdgeLength(String parentNode, String childNode) {
if (graph != null) {
for (ch8p2_edge s : graph) {
if (s.getParentNode() == parentNode
&& s.getChildNode() == childNode)
return s.getEdgeLength();
}
}
return -1;
}
public static ch8p2_minpath getMinSideNode() {
// Create a minimum shortest path object
ch8p2_minpath minMsp = null;
// while the solved node ArrayList is greater than zero
if (solvedConn.size() > 0) {
// Create an index value set to zero
int index = 0;
// for each value in the solved nodes ArrayList
for (int j = 0; j < solvedConn.size(); j++) {
// Create a shortest path map get the
// MinPath of the solved node
ch8p2_minpath msp = getMinPath(solvedConn.get(j));
// if there is no minimum shortest path, // if the minimum shortest
// path
// does not equal -1
// AND the minimum shortest path get
// weight is less than the
// minimum shortest path get weight
if (minMsp == null || msp.getEdgeLength() != -1
&& msp.getEdgeLength() < minMsp.getEdgeLength()) {
// set the minimum shortest path to // the minimum shortest
// path
minMsp = msp;
// set the index value equal it the // node value j
index = j;
}
}
// remove the index that you checked in the
// solved nodes
solvedConn.remove(index);
}
// return the MinShortPath object
return minMsp;
}
public static ch8p2_minpath getMinPath(String node) {
// Create a Minshort Path object that is an ArrayList // and set the take
// in node as the base
// in this case will always be zero
ch8p2_minpath msp = new ch8p2_minpath(node);
// if the parents array does not equal null and the
// unsolved nodes does
// not equal null
if (parents != null && unsolvedConn != null) {
for (int i = 0; i < unsolvedConn.size(); i++) {
ch8p2_minpath tempMsp = new ch8p2_minpath(node);
String parent = unsolvedConn.get(i);
String curNode = node;
while (parent != null) {
int weight = getEdgeLength(parent, curNode);
if (weight > -1) {
tempMsp.addNode(parent);
tempMsp.addEdgeLength(weight);
curNode = parent;
parent = getParent(parents, parent);
} else
break;
}
if (msp.getEdgeLength() == -1 || tempMsp.getEdgeLength() != -1
&& msp.getEdgeLength() > tempMsp.getEdgeLength())
msp = tempMsp;
}
}
return msp;
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch8p2_edge.java
//edge class including the parent node and child node
//and the edge length between two nodes
//******************************************************************
public class ch8p2_edge {
private String parentNode;
private String childNode;
private int edgeLength;
public ch8p2_edge(String parentNode, String childNode, int edgeLength) {
this.parentNode = parentNode;
this.childNode = childNode;
this.edgeLength = edgeLength;
}
public String getParentNode() {
return parentNode;
}
public void setParentNode(String parentNode) {
this.parentNode = parentNode;
}
public String getChildNode() {
return childNode;
}
public void setChildNode(String childNode) {
this.childNode = childNode;
}
public int getEdgeLength() {
return edgeLength;
}
public void setEdgeLength(int edgeLength) {
this.edgeLength = edgeLength;
}
}
//************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch8p2_minpath.java
//the minimum path from starting node to all the other nodes, //including each node
//and the corresponding minimum distance as well
//************************************************************************************
import java.util.ArrayList;
import lejos.robotics.navigation.Waypoint;
import lejos.robotics.pathfinding.Path;
public class ch8p2_minpath {
private ArrayList<String> nodeList;
private int edgeLength;
public ch8p2_minpath(String node) {
nodeList = new ArrayList<String>();
nodeList.add(node);
edgeLength = -1;
}
public ArrayList<String> getNodeList() {
return nodeList;
}
public void setNodeList(ArrayList<String> nodeList) {
this.nodeList = nodeList;
}
public void addNode(String node) {
if (nodeList == null)
nodeList = new ArrayList<String>();
nodeList.add(0, node);
}
public String getLastNode() {
int size = nodeList.size();
return nodeList.get(size - 1);
}
public int getEdgeLength() {
return edgeLength;
}
public void setEdgeLength(int edgeLength) {
this.edgeLength = edgeLength;
}
public void outputPath() {
outputPath(null);
}
public Path buildPath(Waypoint[] coordinates) {
Path result = new Path();
int p = 0;
for (int i = 0; i < nodeList.size(); i++) {
if (nodeList.get(i).equals("A"))
p = 0;
else if (nodeList.get(i).equals("A"))
p = 0;
else if (nodeList.get(i).equals("B"))
p = 1;
else if (nodeList.get(i).equals("C"))
p = 2;
else if (nodeList.get(i).equals("D"))
p = 3;
else if (nodeList.get(i).equals("E"))
p = 4;
else if (nodeList.get(i).equals("F"))
p = 5;
else if (nodeList.get(i).equals("G"))
p = 6;
else if (nodeList.get(i).equals("H"))
p = 7;
else if (nodeList.get(i).equals("I"))
p = 8;
result.add(coordinates[p]);
}
return result;
}
public void outputPath(String pathNode) {
String result = "the munimum path of ";
if (pathNode != null)
nodeList.add(pathNode);
for (int i = 0; i < nodeList.size(); i++) {
result += "" + nodeList.get(i);
if (i < nodeList.size() - 1)
result += "->";
}
result += " is:" + edgeLength;
System.out.println(result);
}
public void addEdgeLength(int e) {
if (edgeLength == -1)
edgeLength = e;
else
edgeLength += e;
}
}摘要
本章介绍了 Dijkstra 搜索算法背后的基本思想和基本原理。你现在能够在实践中应用 Dijkstra 算法来解决路径规划问题。特别是，本章介绍了如何基于 Dijkstra 的路径规划算法和导航类来构建一个问题解决智能体，在该导航类中，问题解决智能体可以智能地找到从起点到任何目的地的路线路径，覆盖尽可能最短的距离。
在下一章中，您将看到如何基于 A* 路径规划算法和导航类构建一个问题解决代理，其中问题解决代理可以智能地在迷宫中找到从起点到任何目的地的路线路径。
九、A* 搜索算法及其在 Lego Mindstorms 中的实现
本章将向你介绍 A* 搜索算法的基础。学完本章后，你将能够在实践中应用 A* 算法解决路径规划问题。您还将看到如何基于 A* 路径规划算法和导航类来构建问题解决代理，其中问题解决代理可以在迷宫中智能地找到从起点到任何目的地的路线路径。
特别是，本章将涵盖以下主题:
- 什么是 A* 算法？
- A* 搜索策略的基本思想。
- 使用 A* 算法进行路径规划的编程实践。
什么是 A* 算法？
A-Star(或 A* )是一种众所周知的搜索算法，与其他搜索算法相比，它在解决路径发现的效率方面极具竞争力。特别是，A* 算法最适合用于那些可以表示为状态空间的问题，例如，在迷宫中探索路径。给定一个合适的问题，问题的初始条件可以用合适的初始状态来表示，目标条件可以表示为目标状态。
对于您执行的每个操作，A* 算法都会生成后续状态来表示该操作的结果。如果你一直这样做，并且在某个时刻生成的后续状态之一是目标状态，那么从初始状态到目标状态的路径就是你的问题的解决方案。而且 A* 算法以一定的方式生成和处理后继状态；也就是说，每当它寻找下一个状态继续时，A* 算法采用启发式函数来尝试挑选下一个要处理的最佳状态。
A* 搜索策略的基本思想
如图 9-1 所示，假设你要从节点 1 到节点 5，有一堵墙把节点 1 和节点 5 隔开。搜索区域被分割成一个基于正方形的网格，您将试图找到的路径是从节点 1 到节点 5 要走哪些正方形。在整个搜索区域中有 15 个方块，标记为 1 到 15。左上角是起点 1，右上角是终点 5，节点 3 和 8 位于中心来表示墙。
图 9-1。
The search area of the A* Algorithm
一旦将搜索区域简化为预定义数量的节点，下一步就是进行搜索，以找到从起点节点 1 到终点节点 5 的最短路径，这是最终目标。从节点 1 开始，检查其相邻的方块，然后通常向外搜索，直到找到目标节点 5。
通过执行以下操作开始 A* 搜索:
- 从起点 1 开始，将其添加到一个“开放列表”中，该列表包含沿着您想要选择的路径的方块。基本上，这是一个需要检查的方块列表。比如OpenList = {1}。
- 查看与起点 1 相邻的所有可到达的正方形，忽略有墙的正方形，并将它们添加到开放列表中。对于这些方块中的每一个，将节点 1 保存为其“父方块”:即OpenList = {1,2,6}。
- 从开放列表中删除起点方块 1，然后将其添加到方块的“封闭列表”中，现在您不需要再查看它。于是就有了OpenList = {2,6}和ClosedList = {1}。
- 所有相邻的方块现在都在要检查的方块的开放列表中。
接下来你需要在开放列表中选择一个相邻的方块继续。但是，有一个问题，你选择哪个广场？答案是它是 F 成本最低的一个，其中 F 是路径得分，这是决定使用哪些方块的关键。
F 是 G 和 H 的和(F = G + H ),其中 G 是沿着生成的路径从起始节点 1 移动到网格上的给定方块的成本，H 是从网格上的那个方块移动到最终目的地(即结束节点 5)的估计成本。启发式函数 H 包括通过反复检查你的开放列表并选择 F 分数最低的方块来生成你的路径。
首先让我们更仔细地看看你是如何计算这个方程的。
- F = G + H
g 是使用生成的路径从起始节点 1 移动到给定方块的成本。在本例中，您为移动的每个水平或垂直方块分配 1 的成本。因此，您有以下内容:
- G(1->2)=1
- G(1->6)=1
启发式函数 H 可以用多种方式来估计。在此示例中，使用了曼哈顿方法，您可以计算从当前方块水平和垂直移动到目标方块的方块总数，忽略对角线移动和可能存在的任何障碍。结果，你有了启发式函数 H:
- H(2->5)=3
- H(6->5)=5
f 是 G 和 h 相加计算出来的。
- F(2)=1+3=4
- F(6)=1+5=6
接下来，如图 9-2 所示，你只需从开放列表的现有节点中选择最低 F 值的方块，即节点 2。然后从开放列表中删除节点 2，并将其添加到封闭列表中，即OpenList = {6}和ClosedList = {1,2}。
图 9-2。
Search from Node 1 to Node 2
检查节点 2 的所有相邻方块。忽略那些在封闭列表中的或不可达的(节点 3 不可达，因为它属于墙)，如果方块不在这个列表中，将它们添加到开放列表中。使所选方块成为新方块的“父方块”。因此，您会得到以下内容:
-
OpenList = {6,7}in which 6’s parent is Node 1 and 7’s parent is Node 2, as shown in Figure 9-3.
图 9-3。
Tree structure of nodes after visiting Node 2
-
ClosedList = {1,2}
现在开放列表中有两个节点(节点 6 和节点 7)。然后计算 F 的值，然后选择 F 值最低的一个。
- OpenList = {6,7}
- ClosedList = {1,2}
在这种情况下，节点 6 和节点 7 具有相同的 F 值 6。那么你选择哪个呢？出于速度的考虑，选择 H 值较小的节点(即节点 7)可能会更快。然后你只需从开放列表上的现有节点中选择最低的 F 得分方块，即节点 7，并将节点 7 从开放列表中删除并将其添加到封闭列表中，即OpenList = {6} and ClosedList = {1,2,7}。
接下来，检查节点 7 的所有相邻方块。忽略那些在封闭列表上或不可达的方块(节点 8 不可达)，如果方块还不在这个列表上，就将它们添加到开放列表中。将选中的方块作为新方块的“父方块”，如图 9-4 所示。因此，您会得到以下内容:
图 9-4。
Tree structure of nodes after visiting Node 7
- OpenList = {6,12}
- ClosedList = {1,2,7}
在这种情况下，节点 6 已经在开放列表中。然后，您需要检查到节点 6 的当前路径是否是更好的路径。使用当前路径1->2->7，您可以计算节点 6 的当前 G 值G(1->2->7->6)，这是 3，然后将其与之前的 G 值G(1->6)，这是 1 进行比较。如果当前的 G 值低于前一个值，则将节点 6 的父节点更改为节点 7。如果不低于上一条，就不做单子上的事。由于先前的 G 分数(1->6)小于当前的 G 分数(1->2->7->6)，所以你不必做列表上的任何事情。
接下来，计算节点 6 和节点 12 的函数 F:
- 你已经知道了F(6) = 6
- H(12->5)=5
- G(1->12)=3
这样你就有了F(12) = 8，它比F(6)大。然后选择节点 6 作为下一个节点，您可以从开放列表中删除节点 6，并将其添加到封闭列表中，如下所示:
- OpenList = {12}
- ClosedList = {1,2,7,6}
节点 6 的父节点是 1，如图 9-5 所示。
图 9-5。
Tree structure of nodes in which Node 6 is chosen as the next node
检查节点 6 的所有相邻方块。忽略那些在封闭列表中或不可及的方块，如果它们不在这个列表中，将它们添加到开放列表中。将选中的方块作为新方块的“父方块”，如图 9-6 所示。因此，您会得到以下内容:
图 9-6。
Tree structure of nodes after visiting Node 6
- OpenList = {12,11}
- ClosedList = {1,2,7,6}
接下来，计算节点 11 和节点 12 的函数 F:
- 你已经知道了F(12) = 8
- H(11->5)=6
- G(1->11)=2
于是就有了F(12) = 8，和F(11)一样。那么你选择哪个呢？类似地，出于速度的目的，选择具有较小 H 分数的节点(即节点 12)可能会更快。
然后选择节点 12 作为下一个节点，从开放列表中删除节点 12，并将其添加到封闭列表中，这样就有了以下内容:
- OpenList = {11}
- ClosedList = {1,2,7,6,12}
节点 12 的父节点是 7，如图 9-7 所示。
图 9-7。
Tree structure of nodes in which Node 12 is chosen as the next node
检查节点 12 的所有相邻方块。忽略那些在封闭列表中或不可及的方块，如果它们不在这个列表中，将它们添加到开放列表中。将选中的方块作为新方块的“父方块”，如图 9-8 所示。
图 9-8。
Tree structure of nodes after visiting Node 12
- OpenList = {11,13}
- ClosedList = {1,2,7,6,12}
接下来，计算节点 11 和节点 13 的函数 F:
- 你已经知道了F(11) = 8
- H(13->5)=4
- G(1->13)=4
于是就有了F(13) = 8，和F(11)一样。那么你选择哪个呢？类似地，出于速度的目的，选择具有较小 H 分数的节点(即节点 13)可能会更快。
然后选择节点 13 作为下一个节点。您从开放列表中删除节点 13，并将其添加到封闭列表中，如下所示:
- OpenList = {11}
- ClosedList = {1,2,7,6,12,13}
节点 13 的父节点是 12，如图 9-9 所示。
图 9-9。
Tree structure of nodes in which Node 13 is chosen as the next node
检查节点 13 的所有相邻方块。忽略那些在封闭列表中或不可及的方块，如果它们不在这个列表中，将它们添加到开放列表中。将选中的方块作为新方块的“父方块”，如图 9-10 所示。
- OpenList = {11,14}
- ClosedList = {1,2,7,6,12,13}
图 9-10。
Tree structure of nodes after visiting Node 13
接下来，计算节点 11 和节点 14 的函数 F:
- 你已经知道了F(11)=8
- H(14->5)=5
- G(1->14)=3
于是你就有了F(14) = 8，和F(11)一样。那么你选择哪个呢？类似地，出于速度的目的，选择具有较小 H 分数的节点(即节点 14)可能会更快。
然后选择节点 14 作为下一个节点。您从开放列表中删除节点 14，并将其添加到封闭列表中，如下所示:
- OpenList = {11}
- ClosedList = {1,2,7,6,12,13,14}
节点 14 的父节点是 13。
检查节点 14 的所有相邻方块。忽略那些在封闭列表中或无法到达的方块，如果方块 9 和 15 不在列表中，将它们添加到开放列表中。使所选方块成为新方块的“父方块”。
- OpenList = {11,9,15}
- ClosedList = {1,2,7,6,12,13,14}
接下来，计算节点 11、节点 9 和节点 15 的函数 F。
- 你已经知道了F(11) = 8
- 按照我们之前的思路，可以算出F(9) = 8和F(15) = 8，和F(11)一样。
你选择哪个？类似地，出于速度的目的，选择具有较小 H 分数的那个可以更快。然后选择节点 15 作为下一个节点，从开放列表中删除节点 15，并将其添加到封闭列表中，如下所示:
- OpenList = {11,9}
- 15 的父母是 14
检查节点 15 的所有相邻方块。忽略那些在封闭列表中或不可及的方块，如果它们不在这个列表中，将它们添加到开放列表中。使所选方块成为新方块的“父方块”。
- OpenList = {11,9,10}
- ClosedList = {1,2,7,6,12,13,14,15}
接下来，计算节点 11、节点 9 和节点 10 的函数 F。
- 你有F(11)=F(9)=F(10)=8
类似地，出于速度的目的，选择具有较小 H 分数的节点(即节点 10)可能会更快。然后选择节点 10 作为下一个节点，从开放列表中删除节点 10，并将其添加到封闭列表中，如下所示:
- OpenList = {11,9,5}
- 10 的父代是 15
找到目标状态节点 5，并在那里停止。因此，最短路径位于根据父节点的封闭列表中，如下所示:
1->2->7->12->13->14->15->10->5 使用 A* 算法进行路径规划的实施规程
在迷宫中寻找路径是一个有趣的问题，通过计算机已经很大程度上解决了这个问题。这个练习的目标是在尽可能短的时间内解决一个简单的迷宫，从一个点开始到另一个点，不管这两点之间有什么障碍。如图 9-11 所示，您的机器人将探索从蓝色圆圈开始到绿色椭圆形结束的路径。平面是一个尺寸为 90 英寸乘 90 英寸的正方形:也就是说，H 和 I 之间的距离是 90 英寸。
图 9-11。
Maze map
从图 9-11 中的地图可以看到:
- A 点和 B 点之间的距离是 10 英寸。
- A 点和 H 点之间的距离是 20 英寸。
- A 点和 I 点之间的距离是 70 英寸。
- B 点和 C 点之间的距离是 50 英寸。
- C 点和 D 点之间的距离是 20 英寸。
- J 点和 E 点之间的距离是 20 英寸。
- E 点和 K 点之间的距离是 40 英寸。
- K 点和 L 点之间的距离是 20 英寸。
- L 点和 M 点之间的距离是 20 英寸。
为了允许各种各样的迷宫解决方法，迷宫的起点和终点将被提前知道，如图 9-11 所示，并且不会有循环。在真实的演示中，您将使用蓝色标记 A，以标识点 A 为起点(在笛卡尔坐标系中，例如，您可以说 A 的坐标是(0,-20))。此外，您将使用绿色来标记线 A 到 H，以便您的机器人可以在到达绿色目的地 B)时停止。在地图中，所有的交叉点都是直角。黑色线条代表墙壁周围。
总而言之，在本练习中，您将创建一个迷宫旅行机器人，它可以探索地图上从蓝色区域开始到绿色区域结束的路径。而且，你的机器人可以检测到终点区域的绿色线，然后在找到最终目的地时通过识别它停下来。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0 ch9p1.java
//simple line map and grid using A\* search build into LeJOS
//******************************************************************
import lejos.geom.*; //used for rectangle
import lejos.robotics.RegulatedMotor; //motor controller
import lejos.robotics.localization.*; //numbers
import lejos.robotics.mapping.LineMap; //mapping
import lejos.robotics.navigation.*; //navigation used for the
// waypoints
import lejos.robotics.pathfinding.*; //A\* search algorithm
import lejos.util.PilotProps; //not used really
import lejos.nxt.Sound;
import lejos.robotics.navigation.DifferentialPilot;
public class ch9p1 {
private static final short[] note = { 2349, (115 / 3), 0, (5 / 3), 1760,
(165 / 3), 0, (35 / 3), 1760, (28 / 3), 0, (13 / 3), 1976,
(23 / 3), 0, (18 / 3), 1760, (18 / 3), 0, (23 / 3), 1568, (15 / 3),
0, (25 / 3), 1480, (103 / 3), 0, (18 / 3), 1175, (180 / 3), 0,
(20 / 3), 1760, (18 / 3), 0, (23 / 3), 1976, (20 / 3), 0, (20 / 3),
1760, (15 / 3), 0, (25 / 3), 1568, (15 / 3), 0, (25 / 3), 2217,
(98 / 3), 0, (23 / 3), 1760, (88 / 3), 0, (33 / 3), 1760, (75 / 3),
0, (5 / 3), 1760, (20 / 3), 0, (20 / 3), 1760, (20 / 3), 0,
(20 / 3), 1976, (18 / 3), 0, (23 / 3), 1760, (18 / 3), 0, (23 / 3),
2217, (225 / 3), 0, (15 / 3), 2217, (218 / 3) };
static RegulatedMotor leftMotor; // motors
static RegulatedMotor rightMotor;
public static void main(String[] args) {
// set up the robot
PilotProps pp = new PilotProps();
float wheelDiameter = Float.parseFloat(pp.getProperty(
PilotProps.KEY_WHEELDIAMETER, "2.11"));
float trackWidth = Float.parseFloat(pp.getProperty(
PilotProps.KEY_TRACKWIDTH, "5.45"));
RegulatedMotor leftMotor = PilotProps.getMotor(pp.getProperty(
PilotProps.KEY_LEFTMOTOR, "C"));
RegulatedMotor rightMotor = PilotProps.getMotor(pp.getProperty(
PilotProps.KEY_RIGHTMOTOR, "A"));
leftMotor.setSpeed(750);
rightMotor.setSpeed(750);
leftMotor.setAcceleration(1000);
rightMotor.setAcceleration(1000);
boolean reverse = Boolean.parseBoolean(pp.getProperty(
PilotProps.KEY_REVERSE, "false"));
// new robot object using the setup
DifferentialPilot robot = new DifferentialPilot(wheelDiameter,
trackWidth, leftMotor, rightMotor, reverse);
// make the robot move faster this is over max
// robot.setTravelSpeed(500);
robot.setRotateSpeed(600);
// Create final map:
Line[] lines = new Line[8]; // six lines inside the map
lines[0] = new Line(-2.5f, -2.5f, -2.5f, 67.5f);
// line AG
lines[1] = new Line(-5.0f, -2.5f, -5.0f, 67.5f);
lines[2] = new Line(-2.5f, 67.5f, 47.5f, 67.5f);
// line GF
lines[3] = new Line(-2.5f, 7.5f, 47.5f, 7.5f);
// line BC
lines[4] = new Line(47.5f, 7.5f, 47.5f, 27.5f);
// line cd
lines[5] = new Line(44f, 7.5f, 44f, 27.5f);
// cd broader
lines[6] = new Line(27.5f, 27.5f, 27.5f, 47.5f);
// line je
lines[7] = new Line(27.5f, 47.5f, 67.5f, 47.5f);
// line ek
lejos.geom.Rectangle bounds = new Rectangle(-22.5f, -2.5f, 90f, 90f);
LineMap myMap = new LineMap(lines, bounds); // add the //bounds to the map
// Use a regular grid of node points. Grid space = 20\. //Clearance = 15:
FourWayGridMesh grid = new FourWayGridMesh(myMap, 10, 2f);
// Use A\* search:
AstarSearchAlgorithm alg = new AstarSearchAlgorithm();
// Give the A\* search alg and grid to the PathFinder:
PathFinder pf = new NodePathFinder(alg, grid);
// store the location of the robot at a given time
PoseProvider posep = new OdometryPoseProvider(robot);
// new navigator loaded with the robot position, and //path
NavPathController nav = new NavPathController(robot, posep, pf);
System.out.println("Planning path…"); // displays as //the path is
calculated
nav.goTo(-12, 0); // goto the end location.
for (int i = 0; i < note.length; i += 2) {
final short w = note[i + 1];
final int n = note[i];
if (n != 0)
Sound.playTone(n, w * 10);
try {
Thread.sleep(w * 10);
} catch (InterruptedException e) {
}
}
}
}摘要
本章介绍 A* 搜索算法的基本原理。学完本章后，你能够在实践中应用 A* 算法解决路径规划问题。具体来说，本章介绍了如何基于 A* 路径规划算法和导航类构建一个问题求解智能体，其中问题求解智能体智能地找到一条从起点到任意目的地的最短距离的路径。
在下一章中，你将学习如何应用一组传感器来执行有根据的动作。特别是，您将使用您创建的轮式机器人，并添加两个传感器——触摸传感器和超声波传感器——以使它在移动时更加了解周围的环境。
链接：https://juejin.cn/post/7399827379051495476
微信分享
评论涨知识
舵机
舵机是一种位置（角度）伺服的驱动器，适用于那些需要角度不断变化并可以保持的控制系统。在高档遥控玩具，如飞机、潜艇模型，遥控机器人中已经得到了普遍应用。
评论:
Scratch 3.0连接EV3
本文介绍如何在Scratch中对EV3机器人进行开发。
Java 机器人编程入门手册（四）
在这一章中，你将学习一组传感器，它们被用来执行有根据的动作。
Java 机器人编程入门手册（二）
这一章向你介绍了在莱霍斯 EV3 使用的笛卡尔坐标系的基础知识。它还教你如何在导航课程中应用编程方法来控制轮式车辆，以便在二维平面中用坐标描绘出预定义的路径。
Java 机器人编程入门手册（一）
本章提供了如何使用乐高 MindStorm EV3 公司建立 Java 机器人编程环境的分步指南，包括乐高 MindStorm EV3 的基本概述和leJOS-EV3的介绍。
乐高EV3遥控车搭建与编程指南
乐高EV3遥控车：从搭建到编程的全面指南
M5 EV3电机底座
Base X 是一款兼容乐高 EV3 电机的专用底座，可同时接入 4 路（RJ11）乐高电机，支持角度 / 速度的读取和控制，完美兼容原有电机功能。
乐高EV3 Java固件leJOS
leJOS是Lego Mindstorms可编程砖的固件替代品。 该软件的不同变体支持原始机器人发明系统，NXT和EV3。
EV3运行程序探究
本文主要探究Bytecode指令集、EV3应用开发与编译、VM运行时等相关内容。
开源：用乐高积木搭建的生物3D打印机，还能打印皮肤组织！
Oliver Castell 博士通过此次开源项目，希望研究人员能够采用这项技术来分享专业知识，并使用额外的 LEGO 组件开发模型，以造福于整个生物医学研究社区。
【EV3研究】EV3概述
乐高 MINDSTORMS Education EV3平台是专为课堂打造的第三代乐高教育机器人技术。
微信分享
