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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

6、

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

8、

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

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

11、

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

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

14、

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

16、將無(wú)人機(jī)的感測(cè)信號(hào)修改為：

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

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

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時(shí)刻到n-1時(shí)刻之間的歷史輸入；bu[n]表示控制輸入向量u[n]和對(duì)應(yīng)權(quán)值系數(shù)矩陣b的乘積，權(quán)值系數(shù)矩陣b根據(jù)控制輸入向量進(jìn)行賦值；

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

24、

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

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

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

28、

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

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

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

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

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

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

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