無人機(jī)與車載系統(tǒng)自動導(dǎo)航核心算法分析

效果預(yù)覽

棕色部分為障礙物,綠色部分為可以行走的地方

一開始標(biāo)記了一個起點和一個終點,用白色表示

點擊計算按鈕后,自動尋找最短的路徑

image

算法分析

image

如圖所示,有起點和中點兩個點,中間棕色部分為障礙物

假設(shè)每一個格子都是正方形,起點移動到黃色格子需要付出的實際代價為10,將實際代價記作 $A$ ,那么此時 $A=10$

又因為是正方形,那么起點到黃色格子上方,或者下方所需要付出的實際代價為 $10\sqrt{2}\approx 14$ ,那么此時 $A=14$

image

如果不考慮障礙物的影響,黃色格子到終點距離還有三個格子,即還需要付出的代價為 $3*10=30?$ ,將還需要付出的代價也叫預(yù)測代價,記作 $P?$ , 此時 $P=30?$ ,

那么黃色格子上方或者下方的兩個格子, $P=4*10=40?$

image

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

注意:為什么不是按照下面路徑來走,計算得到 $P=2*10+14=34$ 呢?

image

理論上是可以的,但是這樣會降低算法的性能,而且代碼也比較繁瑣,這個涉及到哈夫曼距離,比如下圖:

紅色,藍(lán)色,黃色的線長度是相等的,亮點之間固然綠色的線最短,但是計算他的長度需要開根號,因此,如果要使綠色的線長度最短,那么紅色,藍(lán)色,黃色的線長度也必定最短

因此我們選用第一種方案,更加直接

image

每一個格子都會有一個實際代價 $A?$ 和一個預(yù)測代價 $P?$

那么將總代價記作 $T=A+P$

則起點周圍一圈所有的 $A,P,T$ 如下圖所示

image

此時可以看到T最小為40,在起點右邊的那個格子,將這個標(biāo)記成綠色

image

我們來到這個綠色格子,重復(fù)上述的過程,算出這個格子周圍一圈格子的A,P,T,這里我們可以發(fā)現(xiàn),這個格子周圍一圈的黃色部分已經(jīng)計算過了,而起點和障礙物無需計算

因此我們繼續(xù)在剩下的黃色格子中,找到T最小的格子,那么我們可以看到T最小的有兩個格子,為54,在綠色格子的上方和下方

這里二選一,隨便選一個就好,假設(shè)我選擇上圖中綠色格子上方的格子,將這個格子也標(biāo)記成綠色

image

這時我們繼續(xù)重復(fù)之前的步驟,以這個格子為中心,計算出它周圍一圈的A,P,T,

已經(jīng)計算過的黃色格子,起點,終點和灰色格子除外

image

接著,我們不停重復(fù)上述步驟,一直到下圖所示

image

這個樹狀圖,我們從終點開始往起點連接

image

我們可以看到,由紅色箭頭組成的路徑,即為我們所求的路徑

代碼實現(xiàn):

1.定義一個坐標(biāo)類Coord,用來記錄一個個小格子的坐標(biāo)

package com.example.myapplication;

/**
 * @Description: 坐標(biāo)
 */
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.定義一個節(jié)點類Node,用來保存起點,終點和每個路徑節(jié)點的信息

package com.example.myapplication;

/**
 * @Description: 路徑結(jié)點
 */
public class Node implements Comparable<Node> {

    public Coord coord; // 坐標(biāo)
    public Node parent; // 父結(jié)點
    public int a; // A：是個準(zhǔn)確的值厨姚，是起點到當(dāng)前結(jié)點的代價
    public int p; // P：是個估值，當(dāng)前結(jié)點到目的結(jié)點的估計代價

    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: 包含地圖所需的所有輸入數(shù)據(jù)
 */
public class MapInfo
{
    public int[][] map; // 二維數(shù)組的地圖
    public int width; // 地圖的寬
    public int hight; // 地圖的高
    public Node start; // 起始結(jié)點
    public Node end; // 最終結(jié)點
    
    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類用來存儲地圖和節(jié)點的數(shù)據(jù)

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;
    
    /**
     * 當(dāng)?shù)貓D上只有一個起點時,touchFlag為1
     * 當(dāng)?shù)貓D上有起點和終點時,touchFlag為2
     * 沒有點或者重置的時候,touchFlag為0
     */
    public static int touchFlag = 0;
    
    // 存放樹狀節(jié)點信息
    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,注意:數(shù)組和屏幕坐標(biāo)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);
    }

    /**
     * 點擊計算的時候調(diào)用
     * @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>();
    //關(guān)閉列表,用來存放綠色格子的列表
    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);
    }

    /**
     * 判斷新加入的格子以前有沒有在開放列表或是關(guān)閉列表中
     */
    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;
    }

    /**
     * 統(tǒng)計結(jié)果
     */
    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;
        //清空以前的所有內(nèi)容
        openList.clear();
        closeList.clear();
        //起點開始
        openList.add(mapInfo.start);
        //從綠點開始擴(kuò)展
        moveNodes(mapInfo);
    }

    private void moveNodes(MapInfo mapInfo) {
        while (!openList.isEmpty()) {
            if (isCoordInClose(mapInfo.end.coord)) {//找到了終點
                //統(tǒng)計結(jié)果
                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) {//如果能填數(shù)字
                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);
            }
        }
    }
}