導航中路徑規劃模組與演算法
路徑規劃是導航系統的基本能力之一。
熟悉這個模組的目標:
1. 熟悉導航常用的路徑規劃經典演算法,這個在導航系統開發比較成熟後,使用哪種演算法並不是最重要的,關鍵是能滿足效能需求
2. 熟悉有哪些路徑規劃的衡量指標,是最近,最省時間,最省油... 度量指標要根據實際需求來開發,哪些指標最常用?
3. 與地圖資料的關係,分層思想
4. 使用者對路徑規劃的偏好,機器學習能力
導航引擎在得到目的地與自身位置資訊後,就需要根據地圖,計算出最優的路徑。
輸入:目的地、當前位置
輸出:最優路徑,或多條備選路徑
路徑規劃的演算法有哪些?
路徑規劃有很多演算法,在導航中,經常提到的就是A*和Dijkstra演算法。
A*演算法是導航路徑計算中的標準演算法。它比Dijkstra演算法多了一個估算函式,若估算函式為0,A*演算法也就退化為Dijkstra演算法。
但在一般的嵌入式硬體上,基於效能和記憶體的限制與要求,不能直接使用A*演算法計算路徑。所以,也有很多改進的方法。
例如:
1. 應用地圖資料分層的思想,簡化地圖中道路的網路結構,也能提高路徑規劃的效能。
2. 起始點與目的地的方向考慮進去,擴充套件時,有方向性進行擴充套件,可以大大減少計算量和儲存空間。
3. 儲存曾經的規劃記錄,也能達到快速檢索的能力。Google的地圖規劃好像就採用的這種思想。
路徑規劃的估計函式或考慮因素有哪些?
最短路徑:只考慮時間,不考慮距離或其他因素
最快路徑:只考慮距離,不考慮時間或其他因素
同時考慮時間和距離因素:50/50的路徑規劃方法。
路徑規劃演算法僅僅是路徑規劃的一小部分,找到能滿足需求的演算法就可以了。
以下程式碼是我在做一個室內導航時,利用Dijkstra演算法,做一個路徑規劃的試驗。
當時對Java不熟悉,程式碼不規範,不過能執行,湊合著看看試驗結果。
在程式碼裡通過,修改評價規則,試驗結果也隨著規則改變。
import java.util.PriorityQueue; import java.util.List; import java.util.ArrayList; import java.util.Collections; class CoordPos { float x; float y; } class Size { float width; //x float height; // y } class Node implements Comparable<Node> { public final int nodeId; public Link[] adjacencies; public double minDistance = Double.POSITIVE_INFINITY; public Node previous; public CoordPos pos = new CoordPos(); public Size objectSize = new Size(); public int NodeType; public Node(int argNodeId) { nodeId = argNodeId; } public String toString() { return String.valueOf(nodeId); } public int compareTo(Node other) // override function, used for priority queue { return Double.compare(minDistance, other.minDistance); } } class Link { public final int nextNodeId; public Node nextNode; public double length; public Link(int argNextNodeId) { nextNodeId = argNextNodeId;} } public class Dijkstra { public static void computePaths(Node source) { source.minDistance = 0.; PriorityQueue<Node> nodeQueue = new PriorityQueue<Node>(); nodeQueue.add(source); while (!nodeQueue.isEmpty()) { Node u = nodeQueue.poll(); // Visit each Link exiting u for (Link e : u.adjacencies) { Node v = e.nextNode; double length = e.length; double distanceThroughU = u.minDistance + length; if (distanceThroughU < v.minDistance) { // evaluation rule nodeQueue.remove(v); // update v v.minDistance = distanceThroughU ; v.previous = u; // link, multi-segment graph nodeQueue.add(v); } } } } public static List<Node> getShortestPathTo(Node target) { List<Node> path = new ArrayList<Node>(); for (Node node = target; node != null; node = node.previous) path.add(node); Collections.reverse(path); return path; } public static void main(String[] args) { Node v0 = new Node(1); v0.pos.x = (float) 0.9; v0.pos.y = (float) 0.6; Node v1 = new Node(2); v1.pos.x = (float) 2.15; v1.pos.y = (float) 0.6; Node v2 = new Node(3); v2.pos.x = (float) 3.4; v2.pos.y = (float) 0.6; Node v3 = new Node(4); v3.pos.x = (float) 0.9; v3.pos.y = (float) 1.8; Node v4 = new Node(5); v4.pos.x = (float) 2.15; v4.pos.y = (float) 1.8; Node v5 = new Node(6); v5.pos.x = (float) 3.4; v5.pos.y = (float) 1.8; Node v6 = new Node(7); v6.pos.x = (float) 0.9; v6.pos.y = (float) 3.0; Node v7 = new Node(8); v7.pos.x = (float) 2.15; v7.pos.y = (float) 3.0; Node v8 = new Node(9); v8.pos.x = (float) 3.4; v8.pos.y = (float) 3.0; v0.adjacencies = new Link[]{ new Link(2) }; // reference v1.adjacencies = new Link[]{ new Link(1), new Link(3), new Link(5) }; v2.adjacencies = new Link[]{ new Link(2) }; v3.adjacencies = new Link[]{ new Link(5), new Link(7) }; v4.adjacencies = new Link[]{ new Link(2), new Link(4), new Link(6), new Link(8) }; v5.adjacencies = new Link[]{ new Link(5), new Link(9) }; v6.adjacencies = new Link[]{ new Link(4), new Link(8) }; v7.adjacencies = new Link[]{ new Link(5), new Link(7), new Link(9)}; v8.adjacencies = new Link[]{ new Link(6), new Link(8) }; Node[] vertices = { v0, v1, v2, v3, v4, v5, v6, v7, v8 }; for (Node v : vertices) { for (Link c : v.adjacencies) { for (Node nextv : vertices) { if (c.nextNodeId == nextv.nodeId) { c.nextNode = nextv; } } 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)); System.out.printf("length: %.2f \n", c.length); } } computePaths(v0); for (Node v : vertices) { System.out.println("Distance to " + v + ": " + v.minDistance); List<Node> path = getShortestPathTo(v); System.out.println("Path: " + path); } } }