[發明專利]一種基于三維模型的無人機路徑規劃方法

有效

• 申請號: 202011057008.3
• 申請日: 2020-09-30
• 公開（公告）號: CN112484726B
• 公開（公告）日: 2022-08-30
• 發明（設計）人: 佘換林;趙滿超;王志敏
• 申請（專利權）人: 天津基點科技有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 天津創展知識產權代理事務所(普通合伙) 12261
• 代理人: 趙曉輝

本發明提供一種基于三維模型的無人機路徑規劃方法，具體步驟如下：步驟一，通過3DVFH+算法計算出候選路徑及各候選路徑點的位置；步驟二，計算出步驟一得到的全部候選路徑點的代價值；步驟三，找出最大的代價值及其對應的候選路徑點，將該候選路徑點選擇為下一個路徑點，并且將上一次選擇的路徑點設置為上上個選擇的路徑點路徑，將當前所在的路徑點設置為上一個候選點，無人機執行當前規劃的路徑后回到步驟一。通過本發明方法對3DVFH+算法的候選路徑點進行更加合理的選擇，能夠在三維場景下使無人機以更加合理的路徑躲避、繞開墻壁等巨大障礙物，且計算速度較快，經對比測試，躲避、繞開墻壁的成功率得到顯著提升。

技術領域

本發明涉及無人機路徑規劃方法技術領域，尤其涉及一種基于三維模型的無人機路徑規劃方法。

背景技術

目前為止PX4 autopilot這樣的開源無人機自動駕駛系統采用的局部路徑規劃依然使用的是3DVFH+的路徑規劃算法，但是在實際測試當中發現此算法對于給定目標點需要無人機躲避繞開墻壁等龐大的平面障礙物到達目標點時，這個算法難以行之有效，很多情況下無人機會在一個較小的范圍內持續震蕩，無法進一步接近目標點，實際測試效果欠佳。

發明內容

本發明旨在解決現有技術的不足，而提供一種基于三維模型的無人機路徑規劃方法。

本發明為實現上述目的，采用以下技術方案：

一種基于三維模型的無人機路徑規劃方法，具體步驟如下：

步驟一，通過3DVFH+算法計算出候選路徑及各候選路徑點的位置；

步驟二，計算出步驟一得到的全部候選路徑點的代價值，代價值由以下公式得出

其中，k_i表示3DVFH+算法得出的第i條候選路徑的候選路徑點的代價值，μ₁，μ₂分別表示公式第一項和第二項的權重，2*μ₂≥|μ₁|≥μ₂，μ₁∈[-100,0)，μ₂∈(0,100]，設pc表示無人機上一次選擇的路徑點計算出的三維移動點位置，pl表示相對于pc的上一次選擇的路徑點計算出的三維移動點位置，p_i表示3DVFH+算法得出的第i條候選路徑所計算出的三維移動點位置，pt表示目標點的三維位置，Δ(x₁,x₂)表示三維位置x₂與x₁的歐式距離，P表示通過所有候選路徑計算出的所有候選位置點的集合，min_x1∈PΔ(x1,x2)表示取每個集合P當中的元素x1與一個x2進行Δ(x₁,x₂)運算并取當中的極小值，max_x1∈PΔx1,x2)表示取每個集合P當中的元素x1與一個x2進行Δ(x₁,x₂)運算并取當中的極大值，初始規劃階段沒有上上個候選點的信息，則代價函數的第一項取值為0；

步驟三，從步驟二的計算結果中找出最大的代價值及其對應的候選路徑點，將該候選路徑點選擇為下一個路徑點，并且將上一次選擇的路徑點設置為上上個選擇的路徑點路徑，將當前所在的路徑點設置為上一個候選點，無人機執行當前規劃的路徑后回到步驟一，如此往復，直到抵達目標點。

進一步的，步驟二中μ₁＝-40，μ₂＝20。

本發明的有益效果是：通過本發明方法對3DVFH+算法的候選路徑點進行更加合理的選擇，能夠在三維場景下使無人機以更加合理的路徑躲避、繞開墻壁等巨大障礙物，且計算速度較快，經對比測試，躲避、繞開墻壁的成功率得到顯著提升。

附圖說明

圖1為本發明執行時的流程圖；