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

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

文章轉自：http://blog.csdn.net/viewcode/article/details/7925987

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

熟悉這個模塊的目標：

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);
        }
    }
}