[發明專利]一種無人機自主尋路避障方法及系統

有效

• 申請號: 201810619904.0
• 申請日: 2018-06-15
• 公開（公告）號: CN108444482B
• 公開（公告）日: 2021-10-22
• 發明（設計）人: 王騫翰;徐博;黃偉;王猛;王鳴曉
• 申請（專利權）人: 東北大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G05D1/10
• 代理公司: 大連東方專利代理有限責任公司 21212
• 代理人: 李馨

1.一種無人機自主尋路避障方法，其特征在于，包括：如下步驟：

S1、采集障礙物的位置信息；

S2、通過柵格法進行三維環境建模，并將其分割成若干個柵格，分別對包含障礙物的柵格和不包含障礙物的柵格進行不同顏色的處理，通過過起始點及目標終點的判斷平面分割柵格，得到二維柵格模型；

S3、基于A*算法在所述二維柵格模型上做出全局靜態路徑規劃，用于尋找起始點與最終點之間的最優的全局參考路徑；

S4、在全局靜態路徑規劃后，進行局部動態路徑規劃，用于在處理完突發的動態環境變化后，盡快回歸至原先規劃好的路徑；

S5、通過貝塞爾曲線對規劃出的線路點進行軌跡跟蹤，完成對路徑軌跡的平滑處理；

動態路徑規劃如下算法所示：

f(n)＝g(n)+h(n)+x(n)

g(n)表示起始點到節點n的路徑代價，g(n)采用一種基于閔可夫斯基距離的公式進行計算，計算各個方向的代價不同從而使得路徑代價不一樣，h(n)表示從節點到目標點的啟發函數；h(n)表示的是曼哈頓距離|x₁-x₂|+|y₁-y₂|；

x(n)表示的是新規劃的節點到原來靜態規劃的路徑的歐氏距離。

2.根據權利要求1所述的方法，其特征在于，所述步驟S1具體包括：

S101、通過傳感器獲取障礙物在傳感器坐標系下的位置信息；

S102、通過坐標變換將所述傳感器坐標系下的位置信息轉換到無人機坐標系下；

S103、通過導航解算將無人機坐標系下的位置信息導入慣性坐標系下，獲得動態的障礙物位置信息。

3.根據權利要求2所述的方法，其特征在于，所述通過過起始點及目標終點的判斷平面分割柵格，得到二維柵格模型具體包括：

S201、過起始點及目標終點，生成垂直于yz平面的判斷平面；

S202、將xy平面的二維柵格映射至所述判斷平面，

當起始點的水平高度高于目標終點的水平高度時：

當起始點的水平高度低于目標終點的水平高度時：

當起始點的水平高度高于目標終點的水平高度一致時，判斷平面平行于xy平面，

P_z＝E_z＝S_z (3)

其中，P₁(P_y,P_z)表示判斷平面上任一點P的坐標在yz平面的投影點，S₁(S_y,S_z)表示起始點S在yz平面的投影點，E₁(E_y,E_z)表示目標終點S在yz平面的投影點，

結合公式(1)(2)(3)得到判斷平面中任一點的坐標，

S203、設判斷平面中某點坐標P_x＝B_x，P_y＝B_y，通過公式(4)判斷二維柵格中的障礙物柵格和自由柵格，其中：

若P_z＞B_z，則平面F上以點P(P_x,P_y,P_z)為中心的柵格為自由柵格，

若P_z＜B_z，則平面F上以點P(P_x,P_y,P_z)為中心的柵格為障礙物柵格，

B(B_x,B_y,B_z)表示三維障礙物柵格坐標。