CodeMaster第三次任務+多線識別&轉角自適應==近通用算法

原文連結 https://bbs.dji.com/thread-231211-1-1.html

首先還是描述一下本次任務要求吧。
我對本次任務的理解是：S1面前有一條通往迷宮的路，進入迷宮的目的是找到其中放置的標籤後開槍擊倒，全部找到後沿出口走出迷宮。出發前迷宮中的路是什麼樣子的不知道，只知道迷宮中有標籤、路、陷阱和出口，各個標籤（包括陷阱、出口）位置和起點位置連線與起點0航向的夾角數據（相當於出發時刻的指南針角度）為已知數據。
這個任務還是很有挑戰性的！！

左邊是迷宮線路施工圖，中間是各標點位置圖，右邊各標籤、陷阱以及終點的方位角圖

任務的具體要求，其中“不能存儲距離信息和地圖其他相關信息"的要求比較含糊，比如：迷宮中的路是直線還是曲線能預先知道嗎？標籤和陷阱都是放置在路的盡頭能預先知道嗎？

實際佈置的迷宮，由於家裡面積有限，無奈裁剪了進口和出口線路距離約20cm，但程序是按原地圖寫的，甲友們在未裁剪的地圖上是可以順利運行進迷宮的第一步程序的（我用局部地圖試驗過了）。

解題的基本算法思路：
1.巡線進迷宮，發現前方有路口時停，開啟依次尋找標籤和出口的循環。
2.尋找每個標籤是一個單獨的無限循環，直到找到後退出循環，開啟下一個標籤識別循環，直到終點。
3.每個無限循環的算法：計算下一個路口距離現在S1中心的實際距離，計算到達下一個路口後應該向那個方向轉動多少度（以更快的逼近存儲的該標籤的方位角），移動到路口後，掃描（識別到標籤後射擊），轉動之前計算的角度，計算下一路口的相關信息。循環進行直到識別到當前標籤。
4.每過幾個路口後、轉動一定角度後調整S1姿態，以優化其在線的左右方向上處於正中間的位置，提高計算下一路口距離、轉角大小的精度。
5.突破題目限制：一是假設迷宮中的路是直線，二是假設標籤是放置在某個胡同的盡頭，在胡同口看的話能看到。
詳細請看以下的流程圖

https://app.liuchengtu.com/#X8afde4f1608779417145ffc1eab892cd，qq登錄的

第一步：我們先看看S1多線識別函數在不同路況下識別出的點位的分佈圖，其中也對比標明了單線識別函數的識別結果

進迷宮看到第一個交點時的分佈圖

若繼續前行，槍口正好擋住交點時的分佈圖

在方格內的識別點位

臨界情況，也是容易出錯的地方，雖然識別出了三條線，但沒有交點

剛過一個十字交點後

到了陷阱拐角附近

第二步：根據流程圖，編寫的程序代碼如下。各函數的作用已經標註的比較詳細了。這裡要說明一點的就是下一個交點處的轉角選定的思路原則：交點處的三條線與標籤的方位角有一個夾角，當夾角越接近90度時，說明這條線將引導S1越快速逼近標籤的方位角上。這裡如何根據切線角、底盤朝向、標籤方位角計算夾角、如何判斷夾角接近90度需要仔細分析，這裡就留給讀者了（下面的程序中寫出了方法，可以倒推原理的）。
程序中把我調試的部分代碼通過註釋的形式也貼了出來，也提醒了大家怎樣做不合適。

程序中尚存在的問題：
1、S1本身對自身x、y方向相對於程序開始運行時的坐標量測精度不高，程序運行時間越長累計誤差越大，據此計算出的底盤中心目前的sita值到識別第4個標籤時可能有20度左右的誤差了（如理論上sita應該為-30度，實際根據x、y計算出來的是-10度），所以程序也做了相應處理。
2、線識別模塊有時不穩定，明明是很好的多線線路確識別成單線或未識別到線，程序增加了多次識別採樣取最長值的方法克服，但有時還是存在問題。
3、由於sita的誤差，存在在掃描第三個標籤的時候把第四個給誤殺了導致第四個標籤無法找到的bug。
4、運行中出現過S1在一個固定範圍沿線路一直轉圈圈的bug，是邏輯問題，有待後續改進。
5、運行中出現過槍口實際下俯10度，控制台的"俯仰"項確為-19度的硬件bug，因為下俯角度不是要求的-20度，故直接導致線識別出現問題，程序崩潰。這時重啟S1就好了。
6、程序專注於問題的解決，為原型模塊，未做簡潔化和加固處理，留下一步優化吧。
7、之前的兩個假設條件實際上還是事前存儲了地圖中的部分信息的，但一時也沒找到什麼好辦法，先就這樣吧，有待進一步優化。

    需要解釋的運行現象：程序採用了角度、距離自計算、自判斷算法，存在每次運行線路不一致的情況，為正常現象，耐心等待，只要線識別函數穩定，一般情況下都能找到所有標籤後從出口出來的。
    甲友們如果發現我的程序算法有bug或可以改善的地方，請不吝賜教。程序中有不清楚的，也可以微信上聯繫討論：cumtzd

為了穩定運行，程序採用0.1m/s的龜速運行，視頻時間較長，所以用了2倍速壓縮時長

最後一項，BUG提交：
1.app中關於多線函數的識別結果信息，第一個參數N官方解釋說是識別的點的數量，但實際上是識別出的線的類型（即1直線、 2Y路口、3十字路口）或者說是識別出的線的數量（1、2、3條），這個應該是個明顯的bug。
2.Python自帶的min、max函數在app的python編程中雖然也被標識為了關鍵字，但並不能用，程序報錯“name 'max' is not defined”，這個應該也是個明顯的bug吧。
3.運行中出現過槍口實際下俯不到10度，雖然要求是下俯20度，控制台的"俯仰"項檢測卻為-19度的硬件bug，我重啟s1硬件後就好了。