无人机与车载系统自动导航核心算法分析

效果预览

棕色部分为障碍物,绿色部分为可以行走的地方

一开始标记了一个起点和一个终点,用白色表示

点击计算按钮后,自动寻找最短的路径

image

算法分析

image

如图所示,有起点和中点两个点,中间棕色部分为障碍物

假设每一个格子都是正方形,起点移动到黄色格子需要付出的实际代价为10,将实际代价记作 $A$ ,那么此时 $A=10$

又因为是正方形,那么起点到黄色格子上方,或者下方所需要付出的实际代价为 $10\sqrt{2}\approx 14$ ,那么此时 $A=14$

image

如果不考虑障碍物的影响,黄色格子到终点距离还有三个格子,即还需要付出的代价为 $3*10=30$ ,将还需要付出的代价也叫预测代价,记作 $P$ , 此时 $P=30$ ,

那么黄色格子上方或者下方的两个格子, $P=4*10=40$

image

(敲黑板,重要的事情要说三遍)

注意:为什么不是按照下面路径来走,计算得到 $P=2*10+14=34$ 呢?

image

理论上是可以的,但是这样会降低算法的性能,而且代码也比较繁琐,这个涉及到哈夫曼距离,比如下图:

红色,蓝色,黄色的线长度是相等的,亮点之间固然绿色的线最短,但是计算他的长度需要开根号,因此,如果要使绿色的线长度最短,那么红色,蓝色,黄色的线长度也必定最短

因此我们选用第一种方案,更加直接

image

每一个格子都会有一个实际代价 $A$ 和一个预测代价 $P$

那么将总代价记作 $T=A+P$

则起点周围一圈所有的 $A,P,T$ 如下图所示

image

此时可以看到T最小为40,在起点右边的那个格子,将这个标记成绿色

image

我们来到这个绿色格子,重复上述的过程,算出这个格子周围一圈格子的A,P,T,这里我们可以发现,这个格子周围一圈的黄色部分已经计算过了,而起点和障碍物无需计算

因此我们继续在剩下的黄色格子中,找到T最小的格子,那么我们可以看到T最小的有两个格子,为54,在绿色格子的上方和下方

这里二选一,随便选一个就好,假设我选择上图中绿色格子上方的格子,将这个格子也标记成绿色

image

这时我们继续重复之前的步骤,以这个格子为中心,计算出它周围一圈的A,P,T,

已经计算过的黄色格子,起点,终点和灰色格子除外

image

接着,我们不停重复上述步骤,一直到下图所示

image

这个树状图,我们从终点开始往起点连接

image

我们可以看到,由红色箭头组成的路径,即为我们所求的路径

代码实现:

1.定义一个坐标类Coord,用来记录一个个小格子的坐标

package com.example.myapplication;

/**
 * @Description: 坐标
 */
public class Coord
{

    public int x;
    public int y;

    public Coord(int x, int y)
    {
        this.x = x;
        this.y = y;
    }

    @Override
    public boolean equals(Object obj)
    {
        if (obj == null) return false;
        if (obj instanceof Coord)
        {
            Coord c = (Coord) obj;
            return x == c.x && y == c.y;
        }
        return false;
    }
}

2.定义一个节点类Node,用来保存起点,终点和每个路径节点的信息

package com.example.myapplication;

/**
 * @Description: 路径结点
 */
public class Node implements Comparable<Node> {

    public Coord coord; // 坐标
    public Node parent; // 父结点
    public int a; // A：是个准确的值，是起点到当前结点的代价
    public int p; // P：是个估值，当前结点到目的结点的估计代价

    public Node(int x, int y) {
        this.coord = new Coord(x, y);
    }

    public Node(Coord coord, Node parent, int a, int p) {
        this.coord = coord;
        this.parent = parent;
        this.a = a;
        this.p = p;
    }

    @Override
    public int compareTo(Node o) {
        if (o == null) return -1;
        if (a + p > o.a + o.p) return 1;
        else if (a + p < o.a + o.p) return -1;
        return 0;
    }
}

3.定义一个地图类MapInfo,用来存储地图信息

package com.example.myapplication;

import android.util.Log;

/**
 * @Description: 包含地图所需的所有输入数据
 */
public class MapInfo
{
    public int[][] map; // 二维数组的地图
    public int width; // 地图的宽
    public int hight; // 地图的高
    public Node start; // 起始结点
    public Node end; // 最终结点
    
    public MapInfo(int[][] map, int width, int hight, Node start, Node end)
    {
        this.map = map;
        this.width = width;
        this.hight = hight;
        this.start = start;
        this.end = end;
        Log.i("MapInfo","初始化地图成功");
    }
}

4.MapUtils类用来存储地图和节点的数据

package com.example.myapplication;


import android.graphics.Path;

import java.util.Stack;

public class MapUtils {

    // 起点的位置
    public static int startRow = 0;
    public static int startCol = 0;
    
    // 终点的位置
    public static int endRow = 0;
    public static int endCol = 0;
    
    /**
     * 当地图上只有一个起点时,touchFlag为1
     * 当地图上有起点和终点时,touchFlag为2
     * 没有点或者重置的时候,touchFlag为0
     */
    public static int touchFlag = 0;
    
    // 存放树状节点信息
    public static Stack<Node> result = new Stack<>();
    public static Path path;

    public final static int WALL = 1; //  障碍
    public final static int PATH = 2; // 路径

    // 0代表可以走的地方,1代表障碍物
    public static int[][] map = {
            {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
            {0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0},
            {0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1},
            {0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1},
            {1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1},
            {1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1},
            {0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0},
            {0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0},
            {1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 1, 1},
            {0, 0, 0, 1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0},
            {0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1},
            {0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1},
            {1, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1},
            {1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 1},
            {0, 0, 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0},
            {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
            {1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, 1, 0},
            {1, 0, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1},
            {1, 0, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 1, 1},
            {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 1, 1}
    };

    public static int mapRowSize = map.length;
    public static int mapColSize = map[0].length;


    /**
     * 打印地图
     */
    public static void printMap(int[][] maps) {
        for (int i = 0; i < maps.length; i++) {
            for (int j = 0; j < maps[i].length; j++) {
                System.out.print(maps[i][j] + " ");
            }
            System.out.println();
        }
    }
}

5.自定义控件,MainAcitivity和布局

自定义控件

package com.example.myapplication;

import android.content.Context;
import android.graphics.Bitmap;
import android.graphics.BitmapFactory;
import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Matrix;
import android.graphics.Paint;
import android.support.annotation.Nullable;
import android.util.AttributeSet;
import android.util.Log;
import android.view.MotionEvent;
import android.view.View;

import static com.example.myapplication.MapUtils.endCol;
import static com.example.myapplication.MapUtils.endRow;
import static com.example.myapplication.MapUtils.map;
import static com.example.myapplication.MapUtils.startCol;
import static com.example.myapplication.MapUtils.startRow;
import static com.example.myapplication.MapUtils.touchFlag;


public class ShowMapView extends View {
    public ShowMapView(Context context) {
        super(context);
    }

    public ShowMapView(Context context, @Nullable AttributeSet attrs) {
        super(context, attrs);
    }

    public ShowMapView(Context context, @Nullable AttributeSet attrs, int defStyleAttr) {
        super(context, attrs, defStyleAttr);
    }

    @Override
    public boolean onTouchEvent(MotionEvent event) {
        float x = event.getX();
        float y = event.getY();
        //每格地图大小为80*80,注意:数组和屏幕坐标X和Y相反
        int row = (int) y / 80;
        int col = (int) x / 80;
        if (map[row][col] == 0) {
            touchFlag++;
            if (touchFlag == 1) {
                startRow = row;
                startCol = col;
                map[row][col] = 2;
            } else if (touchFlag == 2) {
                endRow = row;
                endCol = col;
                map[row][col] = 2;
            }
        }
        this.invalidate();
        return super.onTouchEvent(event);
    }

    @Override
    protected void onDraw(Canvas canvas) {
        Paint paint = new Paint();
        paint.setAntiAlias(true);
        paint.setColor(Color.BLUE);
        paint.setStrokeWidth(5);
        paint.setStyle(Paint.Style.STROKE);
        for (int i = 0; i < map.length; i++) {
            for (int j = 0; j < map[i].length; j++) {
                if (map[i][j] == 0) {
                    Bitmap bm = BitmapFactory.decodeResource(this.getResources(), R.drawable.route);
                    bm = changeBitmapSize(bm, 80, 80);
                    canvas.drawBitmap(bm, j * 80, i * 80, paint);
                } else if (map[i][j] == 1) {
                    Bitmap bm = BitmapFactory.decodeResource(this.getResources(), R.drawable.wall);
                    bm = changeBitmapSize(bm, 80, 80);
                    canvas.drawBitmap(bm, j * 80, i * 80, paint);
                } else if (map[i][j] == 2) {
                    Bitmap bm = BitmapFactory.decodeResource(this.getResources(), R.drawable.path);
                    bm = changeBitmapSize(bm, 80, 80);
                    canvas.drawBitmap(bm, j * 80, i * 80, paint);
                }
            }
        }
        if (MapUtils.path != null) {
            canvas.drawPath(MapUtils.path, paint);
        }
        super.onDraw(canvas);
    }


    /**
     * 将图片大小变成80*80px
     * @param bitmap
     * @param newWidth
     * @param newHeight
     * @return
     */
    private Bitmap changeBitmapSize(Bitmap bitmap, int newWidth, int newHeight) {
        int width = bitmap.getWidth();
        int height = bitmap.getHeight();

        //计算压缩的比率
        float scaleWidth = ((float) newWidth) / width;
        float scaleHeight = ((float) newHeight) / height;

        //获取想要缩放的matrix
        Matrix matrix = new Matrix();
        matrix.postScale(scaleWidth, scaleHeight);

        //获取新的bitmap
        bitmap = Bitmap.createBitmap(bitmap, 0, 0, width, height, matrix, true);
        bitmap.getWidth();
        bitmap.getHeight();
        Log.e("newWidth", "newWidth" + bitmap.getWidth());
        Log.e("newHeight", "newHeight" + bitmap.getHeight());
        return bitmap;
    }
}

MainActivity

package com.example.myapplication;

import android.os.Bundle;
import android.os.Handler;
import android.os.Looper;
import android.support.v7.app.AppCompatActivity;
import android.view.View;

import static com.example.myapplication.MapUtils.endCol;
import static com.example.myapplication.MapUtils.endRow;
import static com.example.myapplication.MapUtils.map;
import static com.example.myapplication.MapUtils.result;
import static com.example.myapplication.MapUtils.startCol;
import static com.example.myapplication.MapUtils.startRow;
import static com.example.myapplication.MapUtils.touchFlag;

public class MainActivity extends AppCompatActivity {

    ShowMapView showMapView;

    Handler handler = null;

    @Override
    protected void onCreate(Bundle savedInstanceState) {

        handler = new Handler(Looper.getMainLooper());
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);
        showMapView = findViewById(R.id.show);
    }

    /**
     * 点击计算的时候调用
     * @param view
     */
    public void cal(View view) {
        MapInfo info = new MapInfo(map, map[0].length, map.length, new Node(startCol, startRow), new Node(endCol, endRow));
        new Astar().start(info);
        new MoveThread(showMapView).start();

    }

    /**
     * 重置地图
     * @param view
     */
    public void reset(View view) {
        MapUtils.path = null;
        touchFlag = 0;
        for (int i = 0; i < map.length; i++) {
            for (int j = 0; j < map[i].length; j++) {
                if (map[i][j] == 2) {
                    map[i][j] = 0;
                }
            }
        }
        showMapView.invalidate();

    }

    /**
     * 白点开始移动的动画由MoveThread完成
     */
    class MoveThread extends Thread {
        ShowMapView showMapView;

        public MoveThread(ShowMapView showMapView) {
            this.showMapView = showMapView;
        }

        @Override
        public void run() {
            while (result.size() > 0) {
                Node node = result.pop();
                map[node.coord.y][node.coord.x] = 2;
                handler.post(new Runnable() {
                    @Override
                    public void run() {
                        showMapView.invalidate();
                    }
                });

                try {
                    Thread.sleep(300);
                } catch (InterruptedException e) {
                    e.printStackTrace();
                }
                map[node.coord.y][node.coord.x] = 0;

            }
            MapUtils.touchFlag = 0;
        }
    }

}

布局文件就很简单了

<?xml version="1.0" encoding="utf-8"?>
<RelativeLayout xmlns:android="http://schemas.android.com/apk/res/android"
                android:layout_width="match_parent"
                android:layout_height="match_parent">

    <com.example.myapplication.ShowMapView
        android:id="@+id/show"
        android:layout_width="match_parent"
        android:layout_height="match_parent"/>

    <Button
        android:id="@+id/btn"
        android:layout_width="wrap_content"
        android:layout_height="wrap_content"
        android:layout_alignParentBottom="true"
        android:onClick="cal"
        android:text="计算"/>

    <Button
        android:id="@+id/reset"
        android:layout_width="wrap_content"
        android:layout_height="wrap_content"
        android:layout_alignParentBottom="true"
        android:layout_toRightOf="@id/btn"
        android:onClick="reset"
        android:text="刷新"/>


</RelativeLayout>

6.核心算法

package com.example.myapplication;

import android.graphics.Path;

import java.util.ArrayList;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Queue;

/**
 * 变种的A星算法
 */
public class Astar {
    public final static int DIRECT_VALUE = 10;
    public final static int OBLIQUE_VALUE = 14;
    //开放列表,用来存放黄色格子的列表
    private Queue<Node> openList = new PriorityQueue<Node>();
    //关闭列表,用来存放绿色格子的列表
    private List<Node> closeList = new ArrayList<Node>();

    //提供算法需要的辅助功能

    /**
     * 计算P值
     */
    private int calcP(Coord end, Coord coord) {
        return Math.abs(end.x - coord.x) + Math.abs(end.y - coord.y);
    }

    /**
     * 判断是否是终点
     */
    private boolean isEndNode(Coord end, Coord coord) {
        return end.equals(coord);
    }

    /**
     * 判断新加入的格子以前有没有在开放列表或是关闭列表中
     */
    private Node findNodeInOpen(Coord coord) {
        if (coord == null || openList.isEmpty()) return null;
        for (Node node : openList) {
            if (node.coord.equals(coord)) {
                return node;
            }
        }
        return null;
    }

    private boolean isCoordInClose(Coord coord) {
        return coord != null && isCoordInClose(coord.x, coord.y);
    }

    private boolean isCoordInClose(int x, int y) {
        if (closeList.isEmpty()) return false;
        for (Node node : closeList) {
            if (node.coord.x == x && node.coord.y == y) {
                return true;
            }
        }
        return false;
    }

    /**
     * 判断一个新的位置是否可以先择
     */
    private boolean canAddNodeToOpen(MapInfo mapInfo, int x, int y) {
        //是否在地图中
        if (x < 0 || x >= mapInfo.width || y < 0 || y >= mapInfo.hight) return false;
        //判断是否不能通过
        if (mapInfo.map[y][x] == MapUtils.WALL) return false;
        //判断是否是黄格子
        if (isCoordInClose(x, y)) return false;
        //可以走的
        return true;
    }

    /**
     * 统计结果
     */
    private void calcPath(int[][] map, Node end) {
        MapUtils.path = new Path();
        if (end != null) {
            MapUtils.path.moveTo(end.coord.x * 80 + 40, end.coord.y * 80 + 40);
        }
        while (end != null) {
            MapUtils.path.lineTo(end.coord.x * 80 + 40, end.coord.y * 80 + 40);
            MapUtils.result.push(end);
            end = end.parent;
        }
    }


    /**
     * 算法开始
     */
    public void start(MapInfo mapInfo) {
        //先判断地图加载是否成功
        if (mapInfo == null) return;
        //清空以前的所有内容
        openList.clear();
        closeList.clear();
        //起点开始
        openList.add(mapInfo.start);
        //从绿点开始扩展
        moveNodes(mapInfo);
    }

    private void moveNodes(MapInfo mapInfo) {
        while (!openList.isEmpty()) {
            if (isCoordInClose(mapInfo.end.coord)) {//找到了终点
                //统计结果
                calcPath(mapInfo.map, mapInfo.end);
                break;
            }
            //开始从open中取一个最小的,放到close中
            Node current = openList.poll();
            closeList.add(current);
            //开始向八个方向上找
            addNeighborNodeInOpen(mapInfo, current);
        }
    }

    private void addNeighborNodeInOpen(MapInfo mapInfo, Node current) {
        int x = current.coord.x;
        int y = current.coord.y;
        //向八个方向找
        addNeighborNodeInOpen(mapInfo, current, x - 1, y, DIRECT_VALUE);
        addNeighborNodeInOpen(mapInfo, current, x, y - 1, DIRECT_VALUE);
        addNeighborNodeInOpen(mapInfo, current, x + 1, y, DIRECT_VALUE);
        addNeighborNodeInOpen(mapInfo, current, x, y + 1, DIRECT_VALUE);

        addNeighborNodeInOpen(mapInfo, current, x + 1, y + 1, OBLIQUE_VALUE);
        addNeighborNodeInOpen(mapInfo, current, x - 1, y - 1, OBLIQUE_VALUE);
        addNeighborNodeInOpen(mapInfo, current, x - 1, y + 1, OBLIQUE_VALUE);
        addNeighborNodeInOpen(mapInfo, current, x + 1, y - 1, OBLIQUE_VALUE);


    }

    private void addNeighborNodeInOpen(MapInfo mapInfo, Node current, int x, int y, int directValue) {
        if (canAddNodeToOpen(mapInfo, x, y)) {//可以通过的路
            Node end = mapInfo.end;
            Coord coord = new Coord(x, y);
            int a = current.a + directValue;
            Node child = findNodeInOpen(coord);
            if (child == null) {//如果能填数字
                int p = calcP(end.coord, coord);
                if (isEndNode(end.coord, coord)) {
                    child = end;
                    child.parent = current;
                    child.a = a;
                    child.p = p;
                } else {
                    child = new Node(coord, current, a, p);
                }
                openList.add(child);
            }
        }
    }
}