本發(fā)明涉及擴(kuò)展卡爾曼濾波以及模型預(yù)測控制的方法，具體涉及一種基于滾動ekf的無人機(jī)路徑實(shí)施規(guī)劃方法。

背景技術(shù)：

1、隨著無人機(jī)(uav)技術(shù)的快速發(fā)展，其在城市空間的應(yīng)用日益增多，包括目標(biāo)追蹤、雷達(dá)測繪、搜索救援等。然而，城市空間環(huán)境復(fù)雜，存在眾多靜態(tài)(如建筑物)和動態(tài)障礙物(如其他無人機(jī))，這給無人機(jī)的安全飛行帶來了嚴(yán)重挑戰(zhàn)。為確保飛行安全，無人機(jī)需要具備有效的自主避障能力。

2、現(xiàn)有的無人機(jī)避障技術(shù)主要包括模型預(yù)測控制(mpc)和勢場法。mpc方法通過預(yù)測障礙物的運(yùn)動軌跡并實(shí)時調(diào)整無人機(jī)的速度來避免碰撞，但隨著時間的推移，該方法可能會遇到數(shù)據(jù)災(zāi)難。勢場法則模擬動物避障行為，通過計(jì)算障礙物周圍的勢場來引導(dǎo)無人機(jī)避障，但該方法在復(fù)雜環(huán)境中可能不夠精確。

3、此外，還有基于碰撞錐方法、分層感知與避讓方法以及快速探索隨機(jī)樹(rrt)等不同技術(shù)的研究。盡管這些方法在特定場景下具有一定的效果，但在動態(tài)且復(fù)雜的城市環(huán)境中，它們的性能可能受限。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明針對現(xiàn)有無人機(jī)避障技術(shù)的局限性，提出一種基于滾動ekf的無人機(jī)路徑實(shí)施規(guī)劃方法，具體包括下列步驟：

2、步驟1：感知獲取環(huán)境信息；

3、無人機(jī)利用其機(jī)載傳感器獲取環(huán)境信息；

4、步驟2：利用ekf進(jìn)行狀態(tài)估計(jì)和軌跡預(yù)測；

5、針對無人機(jī)俯視圖構(gòu)建平面直角坐標(biāo)系，設(shè)坐標(biāo)原點(diǎn)(0，0)為無人機(jī)位置；無人機(jī)真實(shí)軌跡是基于預(yù)測軌跡的不確定曲線；在離散的n時刻，無人機(jī)在x軸和y軸上的速度分量為：

6、

7、其中：ux[n]和uy[n]分別表示對于無人機(jī)的控制輸入在x、y軸上的分量，該控制輸入用于改變無人機(jī)的速度矢量；vx[n]和vy[n]分別為n時刻無人機(jī)在x和y軸上的速度矢量大??；n時刻障礙物位置表示為其與x軸的距離rx[n]以及與y軸的距離ry[n]：

8、

9、其中：δ為小時間間隔，n-1時刻障礙物位置表示為其與x軸的距離rx[n-1]以及與y軸的距離ry[n-1]；障礙物當(dāng)前n時刻的位置rx[n]和ry[n]，表示為上一時刻，也就是n-1時刻的位置rx[n-1]與ry[n-1]，以及在此基礎(chǔ)上疊加的速度矢量vx[n]和vy[n]分別與小時間間隔δ的乘積；

10、在動態(tài)障礙物的離散模型中，假設(shè)動態(tài)障礙物在n-1時刻之前以相同的速度運(yùn)動，但是在n時刻發(fā)生變化；則狀態(tài)向量s[n]表示為無人機(jī)的位置和障礙的位置組合：

11、

12、將上式表達(dá)為：s[n]＝as[n-1]+u[n]，其中：s[n]為n時刻的狀態(tài)向量，a為矩陣系數(shù)、u[n]為控制輸入向量；

13、感測信號是包含無人機(jī)與動態(tài)障礙之間的距離r擾動的wr[n]，以及包含角度無人機(jī)與動態(tài)障礙之間的角度θ擾動的wθ[n]噪聲信息：

14、

15、其中：wr[n]為距離擾動噪聲信息，wθ[n]為角度擾動噪聲信息，為在距離噪聲wr[n]基礎(chǔ)之上的距離感測信號，r[n]為當(dāng)前距離感測信號的實(shí)際值，為在角度噪聲wθ[n]基礎(chǔ)之上的角度感測信號，θ[n]為當(dāng)前角度感測信號的實(shí)際值；

16、將無人機(jī)的感測信號修改為：

17、x[n]＝h(s[n])+w[n]????(5)

18、其中：x[n]和h(s[n])分別表示無人機(jī)感測信號向量和實(shí)際測量值向量，w[n]為擾動量向量，對其定義分別如下式所示：

19、

20、采用ekf函數(shù)的濾波模型為：

21、

22、其中：a(s[n-1])表示ekf濾波模型的歷史輸入函數(shù)，a(s[n-1])＝{s[n-1]，s[n-2]，…，s[n-k]}，表示從n-k時刻到n-1時刻之間的歷史輸入；bu[n]表示控制輸入向量u[n]和對應(yīng)權(quán)值系數(shù)矩陣b的乘積，權(quán)值系數(shù)矩陣b根據(jù)控制輸入向量進(jìn)行賦值；

23、ekf通過泰勒級數(shù)展開將非線性方程轉(zhuǎn)化為線性方程，a(s[n-1])和h(s[n])在n-1時刻的狀態(tài)向量處利用泰勒級數(shù)展開線性化，n時刻無人機(jī)飛行狀態(tài)的雅可比矩陣a[n-1]與h(n)為：

24、

25、其中，a[n-1]表示對于ekf濾波模型的歷史輸入函數(shù)a(s[n-1])進(jìn)行線性化得出的雅可比矩陣，h[n]表示針對測量值h(s[n])線性化得出的雅可比矩陣，其中a為a(s[n-1])的縮寫；表示對n-1時刻無人機(jī)感測信號s[n-1]、以及在s[n-1]基礎(chǔ)上得到的n-1時刻的信息的估計(jì)值；

26、為簡化障礙物，從三維空間中選擇二維水平空間；對動態(tài)障礙物引入時間軸，在三維空間中構(gòu)建時間-障礙動態(tài)地圖todm；

27、在前面所構(gòu)建的俯視圖平面直角坐標(biāo)系中，todm由狀態(tài)<x，y，v，θ，t>組成，其中：x和y分別表示以無人機(jī)位置為原點(diǎn)，動態(tài)障礙物相對于無人機(jī)位置的橫坐標(biāo)與縱坐標(biāo)，v為無人機(jī)的飛行速度，θ為無人機(jī)飛行方向，t為無人機(jī)的飛行時間；在環(huán)境中存在有限數(shù)量的動態(tài)障礙物d_obstaclei＝(xi，yi，vi，θi，t)，其中：在平面直角坐標(biāo)系中，以無人機(jī)為坐標(biāo)原點(diǎn)，第i個動態(tài)障礙物的橫縱坐標(biāo)位置(xi，yi)、速度vi和方向θi；設(shè)todm的更新速率為小時間段δttedm，對于動態(tài)障礙物：

28、

29、表示在小時間段δttedm無限小的情況下，速度vi和方向θi和在小時間段[t，t+δttedm]內(nèi)保持不變，其中分別表示t+δttedm時刻第i個動態(tài)障礙物的速度和方向；則能夠預(yù)測和定位該時間段內(nèi)的運(yùn)動趨勢；

30、無人機(jī)能夠?qū)Ω兄秶鷥?nèi)[t，t+δttedm]時間段的障礙物進(jìn)行預(yù)測，并將其轉(zhuǎn)化為todm；這是因?yàn)樗菚r間段的極小值，所以無人機(jī)只有避開[t，t+δttedm]時間段內(nèi)的障礙物才能避開所有的障礙物；

31、步驟3：模型預(yù)測控制滾動優(yōu)化軌跡；

32、無人機(jī)引入時間軸的概念，在模型預(yù)測控制mpc算法下進(jìn)行滾動優(yōu)化；這一過程將無人機(jī)的飛行時間劃分為多個小的時間間隔，每個時間間隔內(nèi)，無人機(jī)都會基于當(dāng)前的狀態(tài)和預(yù)測的環(huán)境信息，根據(jù)步驟2中的流程，構(gòu)建一個時間-障礙物動態(tài)圖todm；

33、利用這個動態(tài)地圖，無人機(jī)采用廣度優(yōu)先搜索bfs算法來探索所有可能的路徑節(jié)點(diǎn)，確保找到一條避開所有障礙物的安全路徑；隨后，無人機(jī)使用a算法來評估這些路徑，并選擇出最優(yōu)的路徑節(jié)點(diǎn)；

34、無人機(jī)不斷地判斷自己是否已經(jīng)到達(dá)任務(wù)點(diǎn)或目標(biāo)點(diǎn)；如果到達(dá)預(yù)定位置，無人機(jī)將終止算法并執(zhí)行相應(yīng)的任務(wù)；如果還未到達(dá)，無人機(jī)將繼續(xù)執(zhí)行路徑規(guī)劃和優(yōu)化，重復(fù)步驟一到步驟三。

35、本發(fā)明方法首先利用ekf進(jìn)行狀態(tài)估計(jì)和軌跡預(yù)測，以降低傳感器觀測到的軌跡中的噪聲，使預(yù)測軌跡更貼近實(shí)際飛行軌跡。接著，通過在模型預(yù)測控制的框架下引入時間軸，將無人機(jī)的飛行時間劃分為小的時間間隔，并基于這些時間間隔構(gòu)建時間-障礙物動態(tài)圖(todm)。利用廣度優(yōu)先搜索方法探索可行路徑，并應(yīng)用a*算法確定最優(yōu)路徑。本發(fā)明的無人機(jī)路徑實(shí)時規(guī)劃方法不僅提高了狀態(tài)估計(jì)和軌跡預(yù)測的準(zhǔn)確性，增強(qiáng)了避障策略的魯棒性，而且通過結(jié)合todm和路徑規(guī)劃算法，實(shí)現(xiàn)了實(shí)時路徑規(guī)劃和優(yōu)化，確保無人機(jī)在復(fù)雜城市環(huán)境中能夠快速適應(yīng)動態(tài)變化，規(guī)劃出既安全又高效的飛行路線。