[發(fā)明專利]基于改進(jìn)相互速度障礙法的無人機(jī)動態(tài)避障方法

有效

• 申請?zhí)枺?/td>: 202110078490.7
• 申請日: 2021-01-20
• 公開（公告）號: CN112925342B
• 公開（公告）日: 2022-07-01
• 發(fā)明（設(shè)計）人: 高婧;任雪平;譚勵;連曉峰;徐天瀛
• 申請（專利權(quán)）人: 北京工商大學(xué)
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 北京萬象新悅知識產(chǎn)權(quán)代理有限公司 11360
• 代理人: 黃鳳茹

1.一種基于改進(jìn)相互速度障礙法的無人機(jī)動態(tài)避障方法，對相互速度障礙方法進(jìn)行改進(jìn)，在判斷出無人機(jī)與障礙物發(fā)生碰撞之后，重新選擇新的飛行速度方向，避障結(jié)束后重新根據(jù)當(dāng)前無人機(jī)與目標(biāo)點的位置選擇最佳飛行速度和飛行方向；包括以下步驟：

1)獲取任務(wù)區(qū)域的面積大小；獲取無人機(jī)的初始坐標(biāo)、半徑和速度，分別記為Pos_A(Pos_A(x),Pos_A(y))、r_A、V_A；獲取動態(tài)障礙物的初始坐標(biāo)、半徑、速度，分別記為Pos_B(Pos_B(x),Pos_B(y))、r_B、V_B；獲取動態(tài)障礙物的方向；

2)計算得出無人機(jī)初始速度方向、無人機(jī)與障礙物是否會發(fā)生碰撞、無人機(jī)與目標(biāo)點的距離；具體包括：

21)根據(jù)無人機(jī)初始坐標(biāo)和目標(biāo)點坐標(biāo)信息，采用式(1)計算得出無人機(jī)的最佳速度方向；

ang_A＝atan2(Goal_A(y)-Pos_A(y),Goal_A(x)-Pos_A(x))

V_Abest＝(V_Abest(x),V_Abest(y)) 式(1)

其中：ang_A為目標(biāo)點坐標(biāo)與無人機(jī)初始坐標(biāo)的連線與X正半軸的夾角，單位用弧度表示；V_Abest(x)＝V_A*cos(ang_A)，V_Abest(y)＝V_A*sin(ang_A)，V_Abest(x)、V_Abest(y)分別為障礙物最佳速度方向V_Abest在X軸和Y軸上移動的距離；Goal_A(x)、Goal_A(y)分別為目標(biāo)點的X軸與Y軸坐標(biāo)；Pos_A(x)、Pos_A(y)分別為無人機(jī)位置的X軸與Y軸坐標(biāo)；

22)根據(jù)得到的無人機(jī)最佳速度方向V_Abest和障礙物速度V_B、無人機(jī)位置Pos_A、障礙物位置Pos_B的信息，判斷無人機(jī)與障礙物是否會碰撞；包括如下過程：

首先，利用式(2)獲得無人機(jī)最佳速度方向V_Abest與障礙物速度V_B的連線與X正半軸的夾角Gamma的值：

Gamma＝(atan2(V_Abest(y)-V_B(y),V_Abest(x)-V_B(x))) 式(2)

然后，利用式(3)獲取障礙物坐標(biāo)與無人機(jī)坐標(biāo)的連線與X正半軸的夾角Alpha的值：

Alpha＝atan2(Pos_B(y)-Pos_A(y),Pos_B(x)-Pos_A(x)) 式(3)

其中，Gamma和Alpha的單位均用弧度表示；

障礙物與無人機(jī)的距離d的公式通過式4計算得到：

再根據(jù)障礙物與無人機(jī)的距離d和r_B*2的值，判斷無人機(jī)與障礙物是否會碰撞；

23)根據(jù)無人機(jī)當(dāng)前位置與目標(biāo)點的距離，識別無人機(jī)是否到達(dá)目標(biāo)點：

如果無人機(jī)當(dāng)前位置與目標(biāo)點的距離在0到r_A之間，則設(shè)置getgoal變量，用于表示無人機(jī)是否到達(dá)目標(biāo)點；無人機(jī)已到達(dá)目標(biāo)點則設(shè)置getgoal等于1，否則設(shè)置為0；

3)進(jìn)行無碰撞時的無人機(jī)路徑規(guī)劃；

如果飛行過程中不發(fā)生障礙物的碰撞，則無人機(jī)沿著V_Abest的方向飛行到目標(biāo)點，當(dāng)getgoal的值從0變?yōu)?時結(jié)束；

4)進(jìn)行發(fā)生碰撞時的無人機(jī)路徑規(guī)劃：

如果計算出飛行過程中會有碰撞發(fā)生，則無人機(jī)先沿著V_Abest的方向飛行，當(dāng)且僅當(dāng)上一秒無人機(jī)與障礙物的距離大于設(shè)定值和下一秒無人機(jī)與障礙物的距離小于等于設(shè)定值時，無人機(jī)回退到上一秒的位置，并改變無人機(jī)的速度方向飛行；

5)發(fā)生碰撞時的無人機(jī)速度選擇；包括：

情況1.若無人機(jī)坐標(biāo)在障礙物速度延長線的上方或者在障礙物速度延長線上，即在無人機(jī)橫坐標(biāo)處，無人機(jī)的縱坐標(biāo)大于等于障礙物速度延長線所在方程的縱坐標(biāo)，則選擇障礙物的上方方向的速度；

情況2.若無人機(jī)坐標(biāo)在障礙物速度延長線的下方，即在無人機(jī)橫坐標(biāo)處，無人機(jī)的縱坐標(biāo)小于障礙物速度延長線所在方程的縱坐標(biāo)，則選擇障礙物的下方方向的速度；

6)發(fā)生碰撞時的無人機(jī)速度方向；包括：

情況1.當(dāng)無人機(jī)坐標(biāo)在障礙物速度延長線的上方或在障礙物速度延長線上時，無人機(jī)下一秒的速度為：無人機(jī)的轉(zhuǎn)彎角度為(Beta+Alpha)，轉(zhuǎn)彎大小為:其中Beta＝asin((r_A+r_B)/d)；

情況2.當(dāng)無人機(jī)坐標(biāo)在障礙物速度延長線的下方時，無人機(jī)下一秒的速度為：無人機(jī)的轉(zhuǎn)彎角度為(Alpha-Beta)，轉(zhuǎn)彎大小為:

7)避障后的無人機(jī)飛行速度方向更新為V_Abest方向；

根據(jù)當(dāng)前無人機(jī)的位置與目標(biāo)點的位置，重新計算無人機(jī)的最佳速度；

無人機(jī)按照更新的V_Abest方向飛行，使無人機(jī)在改變飛行角度避免障礙物后，沿著目標(biāo)點飛行；

通過上述步驟，實現(xiàn)無人機(jī)飛行過程中基于改進(jìn)相互速度障礙法的無人機(jī)動態(tài)避障。