導航中路徑規劃模塊與算法

路徑規劃是導航系統的基本能力之一。

熟悉這個模塊的目標：

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);  
        }  
    }  
}  </pre><br />

來自：http://blog.csdn.net/viewcode/article/details/7925987