無人機最短路徑搜索方法
【專利摘要】本發明提供了一種無人機最短路徑搜索方法，包括：S1：獲取地圖；依據預設方格邊長劃分所述地圖為方格圖；S2：通過超聲波攝像頭獲取當前路況信息；S3：依據所述當前路況信息，判斷以當前方格為出發點，能夠通過的下一方格，并標記所述下一方格；S4：選擇所述下一方格中與起始點方格到目的地方格直線連接的方向夾角最小的方格進入；S5：返回執行S2，直到所述方格為目的地方格。能夠快速地搜索出到達指定地點的最短路徑。
【專利說明】
無人機最短路徑搜索方法
技術領域
[0001] 本發明涉及無人機技術領域，特別涉及一種無人機最短路徑搜索方法。
【背景技術】
[0002] 目前的無人機的路徑搜索裝置通常僅僅是找到目的地，而不是搜索出到達指定點 的最短路徑。通常尋找最短路徑的方法是根據地圖的復雜程度設置節點，因此其地圖越復 雜，節點設置也就越多，從而尋找最短路徑的算法運算量也就越大。公開號為CN103697896A 的中國專利提出了一種無人機路徑規劃方法，包括：第一步，初始化任務地圖，標注起點 Start(xstart，ystart)、終點Goal(xgoal，ygoal)和障礙物的坐標，計算出所述無人機的最 小安全轉向半徑R;第二步，設定起點為節點Node [0]和終點為節點Node [-1]，查找其它節點 坐標，以結構體建立二叉樹，第三步，以深度優先左值遍歷方法讀取二叉樹，依次記錄節點 順序，將相鄰節點間的連線及采用了Di jkstra算法生成的路徑存入矩陣Path[]之中。該方 案在節點標定方面進行了優化，節省了一定的計算時間和存儲空間，然而，其節點設置仍然 受地圖復雜程度的影響，不能實現快速、高效地搜索出到達指定地點的最短路徑。
【發明內容】
[0003] 本發明所要解決的技術問題是：無人機如何快速搜索出到達指定地點的最短路 徑。
[0004] 為了解決上述技術問題，本發明采用的技術方案為：
[0005] -種無人機最短路徑搜索方法，包括：
[0006] S1:獲取地圖；依據預設方格邊長劃分所述地圖為方格圖；
[0007] S2:通過超聲波攝像頭獲取當前路況信息；
[0008] S3:依據所述當前路況信息，判斷以當前方格為出發點，能夠通過的下一方格，并 標記所述下一方格；
[0009] S4:選擇所述下一方格中與起始點方格到目的地方格直線連接的方向夾角最小的 方格進入；
[0010] S5:返回執行S2,直到所述方格為目的地方格。
[0011] 本發明的有益效果在于:將地圖劃分成方格圖，并根據超聲波攝像頭獲取的當前 路況信息即可判斷下一步能夠進入哪些方格，再根據向心法則，選擇能夠進入的方格中與 起始點方格到目的地方格直線連接的方向夾角最小的方格進入，實現每走一格都是最短路 徑，直到到達到目的地方格，通過本發明實現以快速的、高效的方式搜索出到達目的地的最 短路徑。
【附圖說明】
[0012] 圖1為本發明實施例一的無人機最短路徑搜索方法流程圖；
[0013] 圖2為本發明實施例一的無人機最短路徑搜索裝置結構圖；
[0014] 圖3為本發明實施例二的無人機最短路徑搜索方法的地圖；
[0015] 圖4為本發明實施例二的無人機最短路徑搜索方法的方格圖；
[0016] 圖5為本發明實施例二的無人機最短路徑搜索方法的有墻地圖最終搜索結果；
[0017] 圖6為本發明實施例二的無人機最短路徑搜索方法的無墻地圖最終搜索結果；
[0018] 圖7為本發明實施例三的無人機最短路徑搜索方法的方格序號標記示意圖。
[0019] 標號說明：
[0020] 1、超聲波攝像頭;2、雙目超聲波攝像頭;3、處理器。
【具體實施方式】
[0021] 為詳細說明本發明的技術內容、所實現目的及效果，以下結合實施方式并配合附 圖予以說明。
[0022] 本發明最關鍵的構思在于:對地圖劃分方格，根據實時的路況信息判斷當前所在 方格能夠通過的下一方格，并選擇下一方格中與起始點方格到目的地方格直線連接的方向 夾角最小的方格進入。
[0023] 本發明涉及的技術術語解釋見表1:
[0024] 表 1
[0025]
[0026]請參照圖1，本發明提供了一種
[0027] -種無人機最短路徑搜索方法，包括：
[0028] S1:獲取地圖；依據預設方格邊長劃分所述地圖為方格圖；
[0029] S2:通過超聲波攝像頭獲取當前路況信息；
[0030] S3:依據所述當前路況信息，判斷以當前方格為出發點，能夠通過的下一方格，并 標記所述下一方格；
[0031 ] S4:選擇所述下一方格中與起始點方格到目的地方格直線連接的方向夾角最小的 方格進入；
[0032] S5:返回執行S2,直到所述方格為目的地方格。
[0033]從上述描述可知，本發明的有益效果在于:將地圖劃分成方格圖，并根據超聲波攝 像頭獲取的當前路況信息即可判斷下一步能夠進入哪些方格，再根據向心法則，選擇能夠 進入的方格中與起始點方格到目的地方格直線連接的方向夾角最小的方格進入，實現每走 一格都是最短路徑，直到到達目的地方格，實現以快速的、高效的方式搜索出到達目的地的 最短路徑。
[0034] 進一步的，所述S1還包括：
[0035] S101:將所述方格圖分別存儲為有墻地圖和無墻地圖；標記有墻地圖的所有方格 初始狀態為1;標記無墻地圖的所有方格初始狀態為〇;
[0036] 所述S3具體為：
[0037] S301:依據所述當前路況信息，判斷與當前方格有共同邊的方格是否有墻；
[0038] S302:標記有墻地圖中無墻的方格為0,以及為能夠通過的下一方格;標記無墻地 圖中無墻的方格為能夠通過的下一方格，以及與當前方格相鄰且除所述下一方格外的所有 方格為1;
[0039] S303:對應標記有墻地圖和無墻地圖中所述下一方格的序號為當前方格的序號加 1；
[0040] 所述S4具體為：
[0041 ] S401:判斷有墻地圖中所述下一方格的序號和無墻地圖中所述下一方格的序號是 否相同；
[0042] S402:若相同，分別選擇所述下一方格中與起始點方格到目的地方格直線連接的 方向夾角最小的方格進入；
[0043] 若不相同，分別選擇有墻地圖和無墻地圖中下一方格序號較小者進入。
[0044] 從上述描述可知，將方格圖保存為有墻地圖和無墻地圖，并且依次對能夠進入的 方格進行升序序號標記，根據能夠進入的方格序號的統計選擇下一步走哪個方格，確保搜 索到的路徑最短。
[0045] 進一步的，所述S303中，記錄起始點所在方格的序號為1，通過洪水填充法分別記 錄有墻地圖和無墻地圖當前方格的序號和下一方格的序號，所述下一方格的序號為當前方 格的序號加1。
[0046] 從上述描述可知，通過洪水填充法將起始點方格到目標點方格經過的方格按起始 序號為1升序排列，按照方格的序號統計結果即可迅速找到能夠通過的路徑。
[0047]進一步的，所述S2具體為:通過超聲波攝像頭檢測障礙物以及與障礙物之間的距 離;通過雙目超聲波攝像頭測量無人機的角速度，通過超聲波攝像頭和雙目攝像頭測量障 礙物的形狀和尺寸。
[0048] 從上述描述可知，通過超聲波攝像頭和雙目超聲波攝像頭即可得到當前的路況信 息和無人機自身的飛行信息。
[0049] 進一步的，所述S1之前，進一步包括：
[0050] S01:無人機懸停時，獲取無人機的晃動幅度L;
[00511 S02:記錄無人機的機身寬度D;
[0052] S03:預設方格邊長為4*L+D。
[0053] 從上述描述可知，將方格的邊長設為4*L+D能夠確保無人機能夠通過方格，且避免 方格太大導致搜索不準確。
[0054]進一步的，所述無人機的上、下方向分別設有超聲波攝像頭;左、中、右方向分別設 有雙目超聲波攝像頭;所述無人機的晃動幅度L由無人機上的處理器依據所述超聲波和所 述雙目超聲波攝像頭傳回的數據計算得到。
[0055]從上述描述可知，由超聲波攝像頭和雙目超聲波攝像頭傳回的路況信息以及無人 機自身的飛行信息計算出的無人機晃動幅度L，能夠反映無人機能夠通過的范圍。
[0056]請參照圖2,本發明的實施例一為：
[0057] 一種無人機最短路徑搜索裝置，包括無人機，兩個分別用于監控向上方向和向下 方向的超聲波攝像頭1，以及三個分別監控左、前、右方向的雙目超聲波攝像頭;兩個超聲波 攝像頭1配合用于光流計算;三個雙目攝像頭2用于測量障礙物的形狀和尺寸;所述無人機 包括處理器3，處理器3分別與超聲波攝像頭1和雙目攝像頭2通訊連接，用于接收超聲波攝 像頭1和雙目攝像頭2上傳的數據并根據所述數據進行相應的處理;其中，所述超聲波攝像 頭1包括攝像頭和超聲波模塊，所述攝像頭用于測量無人機的角速度，所述超聲波模塊用于 測量距離。
[0058] 請參照圖3以及圖4，本發明的實施例二為：
[0059] 利用上述實施例一的無人機最短路徑搜索裝置的無人機最短路徑搜索方法，包括
[0060] S1:獲取地圖；依據預設方格邊長劃分所述地圖為方格圖；該步驟還可以進一步包 括：
[0061] S101:將所述方格圖分別存儲為有墻地圖和無墻地圖；標記有墻地圖的所有方格 初始狀態為1;標記無墻地圖的所有方格初始狀態為〇;地圖如圖3所示，將地圖劃分成一個 16* 16的方格圖；如圖4所示，第15行第1列為起始點方格，第7行第8列為目的地方格；
[0062] S2:通過超聲波攝像頭1和雙目超聲波攝像頭2獲取當前路況信息和無人機自身的 飛行信息；具體為:通過超聲波攝像頭1檢測障礙物以及與障礙物之間的距離;通過雙目超 聲波攝像頭2測量無人機的角速度，通過超聲波攝像頭1和雙目超聲波攝像頭2測量障礙物 的形狀和尺寸；
[0063] S301:依據所述當前路況信息，判斷與當前方格有共同邊的方格是否有墻；
[0064] S302:標記有墻地圖中無墻的方格為0,以及為能夠通過的下一方格;標記無墻地 圖中無墻的方格為能夠通過的下一方格，以及與當前方格相鄰且除所述下一方格外的所有 方格為1;如圖4所示，假設當前方格為第14行第6列的方格，相鄰陰影部分的方格互相可通 過，分別對有墻地圖和無墻地圖的下一方格進行標記，則有墻地圖中第13行第6列的方格標 記為0,無墻地圖第14行第5列和第14行第7列的方格標記為1;
[0065] S303:對應標記有墻地圖和無墻地圖中所述下一方格的序號為當前方格的序號加 1;具體的，記錄起始點所在方格的序號為1，通過洪水填充法分別記錄有墻地圖和無墻地圖 當前方格的序號和下一方格的序號，所述下一方格的序號為當前方格的序號加1;例如，如 圖4所示，第15行第1列標記序號為1，第15行第2列標記序號為2,第15行第3列標記序號為3。 [0066] S401:判斷有墻地圖中所述下一方格的序號和無墻地圖中所述下一方格的序號是 否相同；
[0067] S402:若相同，分別選擇所述下一方格中與起始點方格到目的地方格直線連接的 方向夾角最小的方格進入；
[0068]若不相同，分別選擇有墻地圖和無墻地圖中下一方格序號較小者進入；
[0069] S5:返回執行S2,直到所述方格為目的地方格。
[0070] S6、若到達目的點之后，有墻地圖和無墻地圖指示的最短路徑還未統一，那么按照 無墻圖指示的最短路徑去探索最短路徑，直到無墻地圖與有墻地圖指示的路徑相同時，找 到真正的最短路徑。如圖5和圖6所示，圖5為有墻地圖最終的搜索路徑，圖6為無墻地圖最終 的搜索路徑。因為無墻圖中的最短路徑永遠小于等于有墻圖中的最短路徑；當不相等時，說 明在沒有探索的空間可能存在通路使得最短路徑更短，因此就要確認無墻地圖中的這條通 路是否能走通;如果能走通，那么此時有墻圖中也出現了這條通路，否則無墻圖中的這條通 路就消失了。
[0071] 請參照圖7,本發明的實施例三為：
[0072] 上述實施例二的無人機最短路徑搜索裝置的方法中方格序號的標記方法，包括：
[0073] 將起始點方格標記序號為1;獲取到起始點方格的路況信息后，更新有墻地圖和無 墻地圖，得到能夠進入的下一方格，并分別利用洪水填充法計算等高圖，對能夠進入的下一 方格標記序號為2;
[0074]當經過上述實施例二S401和S402的步驟進入序號為2的方格后，獲取當前方格（即 序號為2的方格)的路況信息后，更新有墻地圖和無墻地圖，得到能夠進入的下一方格，并分 別利用洪水填充法計算等高圖，對能夠進入的下一方格標記序號為3;
[0075] 依此類推，每一次獲取到新的方格路況信息后，更新有墻地圖和無墻地圖，并分別 利用洪水填充法計算等高圖，標記經過的方格序號，如圖4所示，相鄰的陰影部分方格表示 互相能通過，序號為3的方格能過的下一方格有兩個，則對所述兩個方格標記序號為4;最終 從起始點方格到目標點方格的序號為1、2、3......
[0076] 本發明實施例四為:上述實施例二的無人機最短路徑搜索裝置的方法中"預設方 格邊長"的計算方法，包括：
[0077] 超聲波攝像頭1和雙目超聲波攝像頭2將實時的路況信息和無人機自身的飛行信 息數據上傳至無人機上的處理器3;
[0078]處理器3根據所述數據計算得到無人機懸停時的晃動幅度L;
[0079]記錄無人機的機身寬度D;
[0080] 預設方格邊長為4*L+D。
[0081] 綜上所述，本發明提供的無人機最短路徑搜索方法，處理器根據超聲波攝像頭和 雙目超聲波攝像頭獲取實時的路況信息和無人機的飛行信息計算出無人機懸停時的晃動 幅度L，再根據L和無人機的機身寬度計算出預設方格邊長，使得預設的方格能夠剛好被無 人機通過，且不會過大導致搜索不準;對地圖按預設方格邊長劃分成方格圖，之后只需對方 格進行判斷選擇下一步進入哪個方格即可，將搜索目標簡單化;通過設置有墻地圖和無墻 地圖，并分別對有墻地圖和無墻地圖中能夠進入的下一個方格標記為當前方格的序號加1， 比較有墻地圖和無墻地圖的下一方格序號是否相同，選擇序號小的下一方格進入，確保選 擇的路徑最短;在多個下一方格中，選擇與起始點方格到目的地方格直線連接的方向夾角 最小的方格進入，確保每一步都是路徑最短，從而實現快速有效的搜索出到達目的地的最 短路徑。
[0082] 以上所述僅為本發明的實施例，并非因此限制本發明的專利范圍，凡是利用本發 明說明書及附圖內容所作的等同變換，或直接或間接運用在相關的技術領域，均同理包括 在本發明的專利保護范圍內。
【主權項】
1. 一種無人機最短路徑搜索方法，其特征在于，包括： S1:獲取地圖；依據預設方格邊長劃分所述地圖為方格圖； S2:通過超聲波攝像頭獲取當前路況信息； S3:依據所述當前路況信息，判斷以當前方格為出發點，能夠通過的下一方格，并標記 所述下一方格； S4:選擇所述下一方格中與起始點方格到目的地方格直線連接的方向夾角最小的方格 進入； S5:返回執行S2，直到所述方格為目的地方格。2. 根據權利要求1所述的無人機最短路徑搜索方法，其特征在于， 所述S1還包括： S101:將所述方格圖分別存儲為有墻地圖和無墻地圖；標記有墻地圖的所有方格初始 狀態為1;標記無墻地圖的所有方格初始狀態為0; 所述S3具體為： S301:依據所述當前路況信息，判斷與當前方格有共同邊的方格是否有墻； S302:標記有墻地圖中無墻的方格為0,以及為能夠通過的下一方格;標記無墻地圖中 無墻的方格為能夠通過的下一方格，以及與當前方格相鄰且除所述下一方格外的所有方格 為1; S303:對應標記有墻地圖和無墻地圖中所述下一方格的序號為當前方格的序號加1; 所述S4具體為： S401:判斷有墻地圖中所述下一方格的序號和無墻地圖中所述下一方格的序號是否相 同； S402:若相同，分別選擇所述下一方格中與起始點方格到目的地方格直線連接的方向 夾角最小的方格進入； 若不相同，分別選擇有墻地圖和無墻地圖中下一方格序號較小者進入。3. 根據權利要求2所述的無人機最短路徑搜索方法，其特征在于，所述S303中，記錄起 始點所在方格的序號為1，通過洪水填充法分別記錄有墻地圖和無墻地圖當前方格的序號 和下一方格的序號，所述下一方格的序號為當前方格的序號加1。4. 根據權利要求1所述的無人機最短路徑搜索方法，其特征在于，所述S2具體為:通過 超聲波攝像頭檢測障礙物以及與障礙物之間的距離;通過雙目超聲波攝像頭測量無人機的 角速度，通過超聲波攝像頭和雙目攝像頭測量障礙物的形狀和尺寸。5. 根據權利要求1所述的無人機最短路徑搜索方法，其特征在于，所述S1之前，進一步 包括： S01:無人機懸停時，獲取無人機的晃動幅度L; S02:記錄無人機的機身寬度D; S03:預設方格邊長為4*L+D。6. 根據權利要求5所述的無人機最短路徑搜索方法，其特征在于，所述無人機的上、下 方向分別設有超聲波攝像頭；左、中、右方向分別設有雙目超聲波攝像頭;所述無人機的晃 動幅度L由無人機上的處理器依據所述超聲波和所述雙目超聲波攝像頭傳回的數據計算得 到。
【文檔編號】G01C21/20GK105973239SQ201610529872
【公開日】2016年9月28日
【申請日】2016年7月6日
【發明人】高建民
【申請人】深圳市高巨創新科技開發有限公司