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

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

熟悉这个模块的目标：

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
{
    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 nodeQueue = new PriorityQueue();
	    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 getShortestPathTo(Node target)
    {
        List path = new ArrayList();
        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 path = getShortestPathTo(v);
            System.out.println("Path: " + path);
        }
    }
}