导航中路径规划模块与算法

导航中路径规划模块与算法

文章转自：http://blog.youkuaiyun.com/viewcode/article/details/7925987

路径规划是导航系统的基本能力之一。

熟悉这个模块的目标：

1. 熟悉导航常用的路径规划经典算法，这个在导航系统开发比较成熟后，使用哪种算法并不是最重要的，关键是能满足性能需求

2. 熟悉有哪些路径规划的衡量指标，是最近，最省时间，最省油... 度量指标要根据实际需求来开发，哪些指标最常用？

3. 与地图数据的关系，分层思想

4. 使用者对路径规划的偏好，机器学习能力

导航引擎在得到目的地与自身位置信息后，就需要根据地图，计算出最优的路径。

输入：目的地、当前位置

输出：最优路径，或多条备选路径

路径规划的算法有哪些？

路径规划有很多算法，在导航中，经常提到的就是A*和Dijkstra算法。

A*算法是导航路径计算中的标准算法。它比Dijkstra算法多了一个估算函数，若估算函数为0，A*算法也就退化为Dijkstra算法。

但在一般的嵌入式硬件上，基于性能和内存的限制与要求，不能直接使用A*算法计算路径。所以，也有很多改进的方法。

例如：

1. 应用地图数据分层的思想，简化地图中道路的网络结构，也能提高路径规划的性能。

2. 起始点与目的地的方向考虑进去，扩展时，有方向性进行扩展，可以大大减少计算量和存储空间。

3. 保存曾经的规划记录，也能达到快速检索的能力。Google的地图规划好像就采用的这种思想。

路径规划的估计函数或考虑因素有哪些？

最短路径：只考虑时间，不考虑距离或其他因素

最快路径：只考虑距离，不考虑时间或其他因素

同时考虑时间和距离因素：50/50的路径规划方法。

路径规划算法仅仅是路径规划的一小部分，找到能满足需求的算法就可以了。

以下代码是我在做一个室内导航时，利用Dijkstra算法，做一个路径规划的试验。

当时对Java不熟悉，代码不规范，不过能运行，凑合着看看试验结果。

在代码里通过，修改评价规则，试验结果也随着规则改变。

[java]  view plain copy

1. import java.util.PriorityQueue;  
2. import java.util.List;  
3. import java.util.ArrayList;  
4. import java.util.Collections;  
5.   
6. class CoordPos  
7. {  
8.     float x;  
9.     float y;  
10. }  
11.   
12. class Size  
13. {  
14.     float width; //x  
15.     float height; // y  
16. }  
17.   
18.   
19.   
20. class Node implements Comparable<Node>  
21. {  
22.     public final int nodeId;  
23.     public Link[] adjacencies;  
24.     public double minDistance = Double.POSITIVE_INFINITY;  
25.     public Node previous;  
26.       
27.     public CoordPos pos = new CoordPos();  
28.     public Size objectSize = new Size();  
29.     public int NodeType;  
30.   
31.     public Node(int argNodeId) { nodeId = argNodeId; }  
32.     public String toString() { return String.valueOf(nodeId); }  
33.   
34.     public int compareTo(Node other)           // override function, used for priority queue  
35.     {  
36.         return Double.compare(minDistance, other.minDistance);  
37.     }  
38.   
39. }  
40.   
41.   
42. class Link  
43. {  
44.     public final int nextNodeId;  
45.     public Node nextNode;  
46.     public double length;  
47.       
48.     public Link(int argNextNodeId)  
49.     { nextNodeId = argNextNodeId;}  
50. }  
51.   
52. public class Dijkstra  
53. {  
54.     public static void computePaths(Node source)  
55.     {  
56.         source.minDistance = 0.;  
57.         PriorityQueue<Node> nodeQueue = new PriorityQueue<Node>();  
58.         nodeQueue.add(source);  
59.   
60.         while (!nodeQueue.isEmpty()) {  
61.             Node u = nodeQueue.poll();  
62.   
63.             // Visit each Link exiting u  
64.             for (Link e : u.adjacencies)  
65.             {  
66.                 Node v = e.nextNode;  
67.                 double length = e.length;  
68.                 double distanceThroughU = u.minDistance + length;  
69.                 if (distanceThroughU < v.minDistance) {            // evaluation rule  
70.                     nodeQueue.remove(v);   // update v  
71.   
72.                     v.minDistance = distanceThroughU ;  
73.                     v.previous = u;    // link, multi-segment graph  
74.                     nodeQueue.add(v);  
75.                 }  
76.             }  
77.         }  
78.     }  
79.   
80.     public static List<Node> getShortestPathTo(Node target)  
81.     {  
82.         List<Node> path = new ArrayList<Node>();  
83.         for (Node node = target; node != null; node = node.previous)  
84.             path.add(node);  
85.   
86.         Collections.reverse(path);  
87.         return path;  
88.     }  
89.   
90.     public static void main(String[] args)  
91.     {  
92.         Node v0 = new Node(1);  
93.         v0.pos.x = (float) 0.9;  
94.         v0.pos.y = (float) 0.6;  
95.         Node v1 = new Node(2);  
96.         v1.pos.x = (float) 2.15;  
97.         v1.pos.y = (float) 0.6;  
98.         Node v2 = new Node(3);  
99.         v2.pos.x = (float) 3.4;  
100.         v2.pos.y = (float) 0.6;  
101.         Node v3 = new Node(4);  
102.         v3.pos.x = (float) 0.9;  
103.         v3.pos.y = (float) 1.8;  
104.         Node v4 = new Node(5);  
105.         v4.pos.x = (float) 2.15;  
106.         v4.pos.y = (float) 1.8;  
107.         Node v5 = new Node(6);  
108.         v5.pos.x = (float) 3.4;  
109.         v5.pos.y = (float) 1.8;  
110.         Node v6 = new Node(7);  
111.         v6.pos.x = (float) 0.9;  
112.         v6.pos.y = (float) 3.0;  
113.         Node v7 = new Node(8);  
114.         v7.pos.x = (float) 2.15;  
115.         v7.pos.y = (float) 3.0;  
116.         Node v8 = new Node(9);  
117.         v8.pos.x = (float) 3.4;  
118.         v8.pos.y = (float) 3.0;  
119.   
120.         v0.adjacencies = new Link[]{ new Link(2) }; // reference  
121.         v1.adjacencies = new Link[]{ new Link(1),  
122.                                      new Link(3),  
123.                                      new Link(5) };  
124.   
125.         v2.adjacencies = new Link[]{ new Link(2) };  
126.         v3.adjacencies = new Link[]{ new Link(5),  
127.                                      new Link(7) };  
128.   
129.         v4.adjacencies = new Link[]{ new Link(2),  
130.                                      new Link(4),  
131.                                      new Link(6),  
132.                                      new Link(8) };  
133.   
134.         v5.adjacencies = new Link[]{ new Link(5),  
135.                                      new Link(9) };  
136.   
137.         v6.adjacencies = new Link[]{ new Link(4),  
138.                                      new Link(8) };  
139.   
140.         v7.adjacencies = new Link[]{ new Link(5),  
141.                                      new Link(7),  
142.                                      new Link(9)};  
143.   
144.         v8.adjacencies = new Link[]{ new Link(6),  
145.                                      new Link(8) };  
146.   
147.         Node[] vertices = { v0, v1, v2, v3, v4, v5, v6, v7, v8 };  
148.         for (Node v : vertices)  
149.         {  
150.             for (Link c : v.adjacencies)  
151.             {  
152.                 for (Node nextv : vertices)  
153.                 {  
154.                     if (c.nextNodeId == nextv.nodeId)  
155.                     {  
156.                         c.nextNode = nextv;  
157.                     }  
158.                 }  
159.                 c.length = Math.sqrt((c.nextNode.pos.x - v.pos.x)*(c.nextNode.pos.x - v.pos.x) + (c.nextNode.pos.y - v.pos.y)*(c.nextNode.pos.y - v.pos.y));  
160.   
161.                 System.out.printf("length: %.2f \n", c.length);  
162.             }  
163.         }  
164.   
165.         computePaths(v0);  
166.         for (Node v : vertices)  
167.         {  
168.             System.out.println("Distance to " + v + ": " + v.minDistance);  
169.             List<Node> path = getShortestPathTo(v);  
170.             System.out.println("Path: " + path);  
171.         }  
172.     }  
173. }