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

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

熟悉这个模块的目标：

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

        }

    }

}