無人機航線規劃思路剖析，基於凸多邊形地塊往複式運動

新媒體管家

簡書作者：CharTen

寫在前面

嗨！很高興看到你點進來閱讀這篇文章，請別介意，標題有點長有點啰嗦（完全是為了seo考慮），但也算是概括了這篇文章的內容。如果你是要開發如下圖所示的場景，但又苦於沒什麼好的思路，那麼這篇文章一定會幫助到你！

往複式運動航線

基於不規則凸多邊形地塊的往複式航線規劃

哦，對了，本文的實現是基於web平台的地圖，使用javascript。如果你也是在web平台上開發，而且任務時間非常緊急，沒有時間閱讀完全文的話。。。我已經將本文的思路封裝成一個庫了，你可以猛戳下面的鏈接，開箱即用：

https://github.com/Char-Ten/cpRPA

兼容各大地圖平台api（其實不同平台api差異的影響很低）哦，不信的話戳demo：

百度地圖demo （https://char-ten.github.io/cpRPA/test/index.baidu.html）

高德地圖demo （https://char-ten.github.io/cpRPA/test/index.lbs.html）

leaflet地圖demo （https://char-ten.github.io/cpRPA/test/index.leaflet.html）

覺得好用的話記得給個star～原創不易，謝謝支持

正文！其實也是套公式

其實這種問題，實際上是數學幾何應用題，既然是數學題啦，那按照考試的套路第一步肯定是套公式啊，這種場景，核心的公式不多，就兩條：

一次函數兩點表達式

繞（tx,ty）點旋轉n度之後縮放SxSy倍的變換矩陣

第一條沒什麼可以說的，初二數學就開始教一次函數的知識，這一條是用來計算航線與地塊邊界的交點的。

第二條就是很經典的複合變換矩陣了，分別是位移矩陣叉乘旋轉矩陣叉乘位移縮放矩陣，我們設其叉乘結果為A，那麼我們就可以列出下面的等式：

計算過程就是。。。橫乘豎橫乘豎橫乘豎橫乘豎橫乘豎橫乘豎橫乘豎橫乘豎。。。。最後化為用於程序的代數式就是：

通過這條公式，就可以計算出航線旋轉後的坐標點。

然後我們把它們分別封裝一下，弄成一個函數調用先：

（左右滑動查看更多）

看到這裡，恭喜你，你已經完成了50%的工作量！如果是在考試，你把這兩條公式列出來，不寫答案也有一半的分數。

先從最簡單的場景開始

一個矩形地塊，航線水平於x軸：

這是一個大概是200*200大小的矩形，左上角的頂點經緯度為nw（西北），右上角的頂點經緯度為ne（東北），右下角的頂點經緯度為se（東南），左下角的頂點經緯度為sw（西南），其中設置無人機飛行的間隔為10。你先不考慮折線的連接順序，就單單考慮一下，每一根橫線如何生成。觀察一下你會發現以下規律：

兩條橫線的間隔是20

每一條橫線都可以表示為y=N，N為常數，表示某個緯度值

每一條橫線段都是y=N與矩形相交產生，也就是每一條橫線段都是該矩形地塊與維度相交的結果

那麼，現在矩形的四個頂點的經緯度是已知的，無人機飛行的間隔也是已知的，這個矩形需要與多少條緯度線相交是未知的，每一條橫線的N是未知的，每一條橫線段左右兩個點的緯度是未知的。根據已知求未知，你的目標已經很明確了，一道很簡單的幾何題：

該矩形需要與多少條緯度線相交：

求每一條橫線的N：

因為矩形的兩條邊是垂直的，所以，橫線段左右兩個點的經度分別為nw.lng，ne.lng。這樣我們就可以繪製出來了：

場景開始變形！

鏘鏘，我們把矩形上面的邊往東挪50米，得到一個平行四邊形：

聰明的你一定發現了，平行四邊形在Y軸上的投影根本沒有發生變化嘛，即使變了之後，穿過地塊的緯度線數目還是不變嘛，只不過，這次因為兩條邊不是垂直的，所以，我們需要計算斜邊與緯度線的交點。等等，你這時候想起了，最開始50%工作量裡面所封裝的那個calcPointInLineWithY函數！

你已經知道斜邊兩個點的坐標，然後你又知道y=N，那你通過一次函數的兩點表達式，完全就可以知道x，也就是經度是多少啦：

那你可以再變一變，讓y軸上的投影也發生變化，就像這樣：

好了，這下你觀察到，每條邊都跟緯度線相交了，也就是說，這次你要遍歷一下這個平行四邊形四個頂點。等等，你似乎忘記了一個問題，這個四邊形在y軸上的投影發生了變化，相交緯度線數目也跟著發生變化了。這時候你想到，要不給這個多邊形做個外接矩形？就像這樣：

這樣是不是又回歸了最開始的場景？只是把calcPointInLineWithY函數加上去之後，你可以得到任意凸多邊形與緯度線相交的模型。

首先是創建多邊形外接的矩形，將多邊形頂點全部遍歷一遍，取到最大和最小的經緯度值。經度最大為東，經度最小為西。緯度最大為北，緯度最小為南。將這幾個組合一下，獲得西北點，東北點，東南點和西南點，就可以弄出一個矩形出來

然後是計算這個外接矩形穿過了多少條緯度線，跟之前那個場景是一樣的。rect是上面那個函數創建的數組，可以看到西北點的索引是0，西南點的索引是3，所以計算西北到西南的點，也就是這個外接矩形的高度。這個方法返回有len條緯度線穿過，而且穿過的緯度相差lat。

最後就是結合上面幾個場景，改寫一下最終的生成函數，這裡你是直接將生成的橫線畫了出來，如果需要做折線連接順序處理，還需要聲明一個數組去存儲生成的點，比如當緯度線的索引是奇數時，橫線從西往東畫，也就是先放西邊的點再放東邊的點；如果緯度線索引是偶數時，橫線從東往西畫，也就是反過來。這裡就留給你自己處理了

到這裡，基本上已經完成了90%，剩下10%那部分最簡單了。簡單么？你歪著頭問，到現在為止只是多邊形與緯度線相交啊，現實中無人機又不可能都是沿著緯度在地圖上橫著飛！它可以在地圖上沿任意方向飛行的，可上面的做法，只能讓無人機橫著走啊！

別急，你這時候要淡定，你要想想開頭不是還留了一個transform函數么？

這個transform的作用是讓坐標(x,y)繞著(tx,ty)旋轉deg度後在縮放SySx倍得到一個的新坐標(_x,_y)。沒錯，這次我們要拿這些坐標進行旋轉運動。

而且牛頓告訴過你：

運動都是相對的

先放一張gif圖，看看如何繪製一個斜45度角的航線：

GIF/458K

先讓多邊形繞著中心點旋轉想要的角度，將得到的新多邊形再與緯度線做相交操作，獲取到那些交點之後，再將那些交點旋轉回來。換句話說，變換前它是一個任意多邊形，變換後，它還是一個任意多邊形，都是滿足上面已經預設好的場景的。這樣的好處顯而易見，你不需要修改上面的任何一個函數，也不需要去多寫一條兩個一次函數求交點的公式。把問題化到最簡單的場景去，只需要添加變換的代碼：

這裡你一定要牢記，lng是經度，對應x；lat是緯度，對應y（被leaflet框架坑哭的我QuQ。。。

然後，將原來的代碼加上去：

小瑕疵

也許細心的你發現了，gif圖裡面轉是轉了，斜45度的角也是畫出來了，但是旋轉後的圖形好像是被拉長了！旋轉回來時又會被壓肥回來。這個嘛。。。

這個問題其實我在寫驗證的時候也發現了，粗略計算多邊形面積和航線掃過的面積的比值，總是發現0度的比值最接近於1，90度的比值最小，不管多邊形是什麼形狀。

最開始我一直以為是面積演算法的原因，直到寫這篇文章的時候去寫那個gif動畫後才發現，多邊形變換後變形了，轉換後與緯度相交的數量變多了。而我猜想變形的原因可能是，地球並不是一個平面，它是彎的你知道吧，這些經緯度雖然是投影到了平面上了，但實際上它們是在一個球上。直接拿經緯度變換相當於先在球上做了旋轉，然後在投影到地圖的平面上，這樣看起來就像是被拉長了。

所以，你不能直接變換經緯度，而是要將經緯度換算成地圖上的像素坐標，變換完之後再轉回來，這樣圖像就不會被拉長了。

因此先來兩個像素系與經緯度坐標系轉換的方法壓壓驚：

然後將原來的createRotatePolygon函數改為：

這樣就解決了拉長的問題

GIF/294K

看到這裡，相信你已經完全掌握了這種思路，即使你是使用iOS或者android的sdk，你也應該可以很快將思路「移植」過去。

至於那個折線的順序，這個只是在push進polyline數組的時候判斷一下i的奇偶修改不同的push順序，很簡單就得到那種折線效果，我相信你是會寫的，這裡就不著筆墨了。更多的源碼請訪問：https://github.com/Char-Ten/cpRPA

寫在最後

終於寫完了此文，真是不容易。這個思路，從最開始思想混亂與Leaflet框架緊密耦合，到一步步解耦，到自己決定寫一個適用於各個地圖平台的庫，到寫這篇文章，差不多已經過去兩個星期了。我發現，很多問題是你調試過程中發現不了的，等到你調試好了，決定寫一篇裝逼的文章，在寫的過程中你就會發現各種調試中出現不了的問題，比如那個變換變形的問題。

在動手前，關於此類的教程文章少之又少，唯一可以找到比較符合場景的竟然是百度文庫裡面的一篇論文：基於作業航向的不規則區域作業航線規劃演算法研究（https://wenku.baidu.com/view/23c7588e9b6648d7c0c74604.html）

論文懂吧，長倒是不長，就是臭，裡面堆砌著各種奇奇怪怪的術語。之後看了兩天後才明白他的實現思路，大同小異，只不過不知道他怎麼搞的，新弄出一個坐標系出來，感覺這樣是增加了思路的複雜度啊。所以在他的演算法的基礎上我做了簡化，不做坐標系偏移，取代的是變換地塊坐標，這樣實現起來相對簡單些。

因此在這裡，小小貢獻一下這個思路吧，也讓以後有人像我這樣被坑去寫這種無人機航線規劃的，能夠很快地實現，不用再去看那些奇奇怪怪的論文了。。。

最後最後最後，安利一下https://github.com/Char-Ten/cpRPA這個庫（已經無恥到這個地步了。。。），可以接入百度、高德、leaflet，然後算航線！什麼？你不需要。。。那你需要計算地圖上多邊形地塊的面積不？我也提供算面積的方法，百度地圖、高德地圖都沒有這種算面積的方法！好評給個star！謝謝大家！（夠了喂！(╯‵′)╯︵┻━┻）

如果你有更好的實現思路，或者新的使用場景，或者有使用問題，或者有bug，歡迎在下面評論。。。或者直接提issue:https://github.com/Char-Ten/cpRPA/issues

GIF/144K

點擊展開全文

喜歡這篇文章嗎？立刻分享出去讓更多人知道吧！