1. 引言

? 無人機(jī)航線規(guī)劃是植保無人機(jī)能否正常作業(yè)的重點(diǎn)，航線規(guī)劃的好壞決定了無人機(jī)能否對(duì)整個(gè)區(qū)域全覆蓋、不飛出邊界、不重復(fù)、不遺漏的掃描噴灑作業(yè)。目前市面上不乏好的植保航線規(guī)劃算法，大多數(shù)是基于凸多邊形的航線規(guī)劃，對(duì)凹多邊形的航線規(guī)劃不能做到全覆蓋、不飛出邊界的要求。下面介紹我所實(shí)現(xiàn)的植保無人機(jī)航線規(guī)劃算法，目的是解決區(qū)域全覆蓋、不飛出邊界等作業(yè)要求，性能上不一定是最優(yōu)實(shí)現(xiàn)。

2. 作業(yè)區(qū)域要求

? 本算法對(duì)作業(yè)區(qū)域沒有過多要求，但對(duì)區(qū)域過于復(fù)雜、作業(yè)面積過小或過大、區(qū)域中有兩條邊線之間夾角過小等不能實(shí)現(xiàn)最優(yōu)。

3. 算法實(shí)現(xiàn)

3.1 區(qū)域旋轉(zhuǎn)

? 在給定的參數(shù)中，選擇的方向可能是任意方向，規(guī)定航線方向可以使航線規(guī)劃更方便。所以整個(gè)區(qū)域在給定的方向需要旋轉(zhuǎn)90°，使給定方向與水平方向平行。目前做法是以整個(gè)區(qū)域的最左上角坐標(biāo)為原點(diǎn)進(jìn)行旋轉(zhuǎn)，給出代碼：

/**
  * 旋轉(zhuǎn)坐標(biāo)系
  *
  * @param boundPoints  需要旋轉(zhuǎn)的坐標(biāo)
  * @param rotateRadian 旋轉(zhuǎn)弧度
  * @return 旋轉(zhuǎn)的數(shù)據(jù)
  * @throws IOException
  * @throws ClassNotFoundException
  */
private static TranslateAndRotateBean translateAndRotateBoundFromWorld(List<? extends PointD> boundPoints, double rotateRadian) throws IOException, ClassNotFoundException {
    MaxMinLatLng maxMinLatLng = getMaxMinLatLng(boundPoints);
    PointD intersectPoint = new PointD(maxMinLatLng.getMinLat(), maxMinLatLng.getMinLng());//旋轉(zhuǎn)后的原點(diǎn)
    double reallyRotateRadian = -rotateRadian + PI12;

    TranslateAndRotateBean translateAndRotateBean = new TranslateAndRotateBean(intersectPoint, reallyRotateRadian);
    //旋轉(zhuǎn)
    for (PointD boundPoint : boundPoints) {
        rotateFromWorld(boundPoint, translateAndRotateBean);
    }
    return translateAndRotateBean;
}

/**
  * 旋轉(zhuǎn)
  *
  * @param pointD 點(diǎn)
  * @param t      旋轉(zhuǎn)參數(shù)
  */
private static void rotateFromWorld(PointD pointD, TranslateAndRotateBean t) {
    double x = pointD.x;
    double y = pointD.y;
    pointD.x = (x - t.getIntersectPoint().x) * cos(t.getReallyRotateRadian()) - (y - t.getIntersectPoint().y) * sin(t.getReallyRotateRadian()) + t.getIntersectPoint().x;
    pointD.y = (y - t.getIntersectPoint().y) * cos(t.getReallyRotateRadian()) + (x - t.getIntersectPoint().x) * sin(t.getReallyRotateRadian()) + t.getIntersectPoint().y;
    }

3.2 生成航線

? 根據(jù)3.1操作，得到了旋轉(zhuǎn)后的區(qū)域，接下來的操作會(huì)針對(duì)旋轉(zhuǎn)后的區(qū)域進(jìn)行航線生成。

start=>start: 開始
1=>operation: 找到整個(gè)區(qū)域中最高點(diǎn)，
            將此點(diǎn)記錄到當(dāng)前最高點(diǎn)
2=>condition: 當(dāng)前最高點(diǎn)是否比最低點(diǎn)大
3=>operation: 根據(jù)給定噴幅從當(dāng)前最高點(diǎn)向
            下移動(dòng)半個(gè)噴幅的距離，
            對(duì)整個(gè)區(qū)域畫橫線
4=>operation: 獲得橫線與區(qū)域的交點(diǎn)，
            將所有區(qū)域交點(diǎn)按照
            從左至右的順序排序
5=>condition: 交點(diǎn)數(shù)量是否大于2個(gè)
6=>operation: 取出前兩個(gè)點(diǎn)，生成
            航線并加入到航線集合中
7=>operation: 移出這兩個(gè)交點(diǎn)
8=>operation: 將當(dāng)前最高點(diǎn)再向
            下移動(dòng)半個(gè)噴幅高度
end=>end: 結(jié)束

start->1
1->2
2(no)->end
2(yes)->3
3->4
4->5
5(yes)->6
5(no)->8
6->7
7->5
8->2

1. 首先找到整個(gè)區(qū)域中最高點(diǎn)，將此點(diǎn)記錄到當(dāng)前最高點(diǎn)；
2. 判斷當(dāng)前最高點(diǎn)是否已經(jīng)比最低點(diǎn)小，如果是則進(jìn)行步驟8，如果否，則進(jìn)行步驟3；
3. 根據(jù)給定噴幅從當(dāng)前最高點(diǎn)向下移動(dòng)半個(gè)噴幅的距離，對(duì)整個(gè)區(qū)域畫橫線；
4. 獲得橫線與區(qū)域的交點(diǎn)，將所有區(qū)域交點(diǎn)按照從左至右的順序排序；
5. 判斷交點(diǎn)數(shù)量是否大于2個(gè)，如果是，進(jìn)行步驟6，如果否進(jìn)行步驟（這里存在一個(gè)問題，待后續(xù)介紹）；
6. 取出前兩個(gè)點(diǎn)，生成航線并加入到航線集合中；
7. 移出這兩個(gè)交點(diǎn)，進(jìn)行步驟4；
8. 將當(dāng)前最高點(diǎn)再向下移動(dòng)半個(gè)噴幅高度，進(jìn)行步驟2；

自此，所有航線都已經(jīng)添加到航線集合中了。

3.3 連接航線

? 首先對(duì)生成的航線按照旋轉(zhuǎn)的角度以及原來的旋轉(zhuǎn)原點(diǎn)對(duì)航線還原成真實(shí)的航線。

? 拿出第一條航線，根據(jù)用戶給定的起飛點(diǎn)的位置，判斷這條航線的開始點(diǎn)、結(jié)束點(diǎn)哪個(gè)距離起飛點(diǎn)更近，如果結(jié)束點(diǎn)距離起飛點(diǎn)更近，則交換開始點(diǎn)、結(jié)束點(diǎn)。

? 取出第二條航線，判斷第二條航線的起飛點(diǎn)與結(jié)束點(diǎn)哪個(gè)與上一條航線的結(jié)束點(diǎn)更近，如果結(jié)束點(diǎn)更近，則交換開始點(diǎn)與結(jié)束點(diǎn)，以此類推。

3.4 小結(jié)

? 此刻，航線已經(jīng)生成，已經(jīng)可以做到全覆蓋，但是對(duì)于某些情況下，無法做到航線飛出區(qū)域外、空飛路徑過長(zhǎng)問題，所以目前生成的航線需要優(yōu)化。

4 航線優(yōu)化

4.1 航線順序優(yōu)化

? 航線生成時(shí)，所有航線都是一個(gè)個(gè)的添加到航線集合中，這樣導(dǎo)致航線在有些情況下，兩條航線的連接會(huì)穿過區(qū)域，使航線在區(qū)域外。所以優(yōu)化之一就是在3.2基礎(chǔ)上，在得到每條航線的交點(diǎn)時(shí)，記錄每條航線與區(qū)域的哪兩條邊相交，將航線關(guān)聯(lián)到兩條邊上，相同的兩條邊上的航線應(yīng)該連接在一起。

/**
* 獲取對(duì)應(yīng)航線區(qū)域
*
* @param sprayGraphList         航線區(qū)域的Map
* @param startLinePointPosition 航線起始交點(diǎn)在區(qū)域中的邊的位置
* @param endLinePointPosition   航線結(jié)束交點(diǎn)在區(qū)域中的邊的位置
* @return 所對(duì)應(yīng)的航線區(qū)域
*/
private static SprayGraph getAssignSprayGraph(Map<String, SprayGraph> sprayGraphList, int startLinePointPosition, int endLinePointPosition) {
    String hashCode = startLinePointPosition + " " + endLinePointPosition;
    SprayGraph sprayGraph = sprayGraphList.get(hashCode);
    if (sprayGraph == null) {
        sprayGraph = new SprayGraph();
        sprayGraphList.put(hashCode, sprayGraph);
    }
    return sprayGraph;
}

SprayGraph類：是真正保存航線的位置。

sprayGraphList：這個(gè)Map中存儲(chǔ)SprayGraph，它的鍵是以航線開始交點(diǎn)的邊的位置和結(jié)束交點(diǎn)邊的位置構(gòu)成的字符串，因?yàn)樯珊骄€時(shí)，航線的開始位置一定會(huì)在航線的結(jié)束位置的左邊，所以不會(huì)出現(xiàn)”1 2“，”2 1“的情況。

4.2 添加A*算法

在4.1中，通過對(duì)航線順序的優(yōu)化，可以做到大部分航線能夠在區(qū)域內(nèi)，但還是有部分航線會(huì)越過區(qū)域邊線飛到區(qū)域之外。所以從一條航線的結(jié)束點(diǎn)到下一條航線的開始點(diǎn)需要在區(qū)域內(nèi)找出最短路徑，目前使用A*算法來達(dá)到不飛到區(qū)域外的情況。關(guān)于A*算法的實(shí)現(xiàn)，將下下一篇文章中介紹。

4.3 減少過短航線

航線過短會(huì)讓無人機(jī)在還沒有加速時(shí)就要開始減速，影響無人機(jī)的作業(yè)效率。計(jì)算航線長(zhǎng)度，判斷航線長(zhǎng)度是否小于給定的最短航線長(zhǎng)度時(shí)，此航線將不加入到航線集合中。

4.4 航線縮進(jìn)

航線縮進(jìn)是為了保證無人機(jī)作業(yè)時(shí)不飛出作業(yè)區(qū)域，更是為了A*算法的實(shí)現(xiàn)提供實(shí)施基礎(chǔ)。航線縮進(jìn)的實(shí)現(xiàn)是以飛機(jī)的半徑為準(zhǔn)，所以航線的縮進(jìn)要保證以飛機(jī)中心點(diǎn)畫圓不能與區(qū)域有交點(diǎn)或僅有一個(gè)交點(diǎn)。