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

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

熟悉这个模块的目标：

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