【技術(shù)實(shí)現(xiàn)步驟摘要】
本專利技術(shù)涉及無人機(jī)自主導(dǎo)航的，具體涉及一種動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法和系統(tǒng)。
技術(shù)介紹
1、隨著無人機(jī)技術(shù)的快速發(fā)展，無人機(jī)在各個(gè)領(lǐng)域得到了廣泛應(yīng)用。為了實(shí)現(xiàn)無人機(jī)的自主飛行，需要無人機(jī)能夠感知周圍的環(huán)境，并根據(jù)環(huán)境信息規(guī)劃安全、高效地飛行軌跡。目前，大多數(shù)無人機(jī)避障算法主要針對(duì)靜態(tài)環(huán)境設(shè)計(jì)。然而，在實(shí)際應(yīng)用中，無人機(jī)往往面臨包含動(dòng)態(tài)障礙物的復(fù)雜環(huán)境，如何在動(dòng)態(tài)環(huán)境下進(jìn)行安全軌跡規(guī)劃是一個(gè)亟需解決的問題。
2、無人機(jī)自主導(dǎo)航在最近的研究中已經(jīng)取得了很大進(jìn)展，然而目前最先進(jìn)的避障算法主要針對(duì)靜態(tài)環(huán)境設(shè)計(jì)。相比于靜態(tài)環(huán)境，動(dòng)態(tài)環(huán)境避障使四旋翼無人機(jī)在感知和規(guī)劃上面臨更大的挑戰(zhàn)：1)對(duì)于深度相機(jī)等對(duì)環(huán)境要求苛刻的傳感器來說，環(huán)境中高速運(yùn)動(dòng)的動(dòng)態(tài)物體往往會(huì)造成運(yùn)動(dòng)模糊和感知誤差的增加，這對(duì)動(dòng)態(tài)障礙物識(shí)別算法的魯棒性和準(zhǔn)確性提出了更高的要求；2)在動(dòng)態(tài)環(huán)境中，路徑規(guī)劃算法必須具有預(yù)見性，從而可以提前規(guī)避未來的可能碰撞，與此同時(shí)還要有更好的實(shí)時(shí)性來應(yīng)對(duì)動(dòng)態(tài)環(huán)境中突然變化；3)所有的感知、預(yù)測和規(guī)劃模塊都需要具有更高的魯棒性和可靠性，以及需要它們之間更合理的配合，以保證整個(gè)系統(tǒng)的穩(wěn)健性。
3、最近已經(jīng)有一些研究來解決動(dòng)態(tài)環(huán)境下的避障問題，如文獻(xiàn)(robust?vision-based?obstacle?avoidance?for?micro?aerial?vehicles?in?dynamic?environments，international?conference?on?robotics?andaut
4、因此，當(dāng)前簡單的預(yù)測動(dòng)態(tài)障礙物運(yùn)動(dòng)并將其加入軌跡規(guī)劃約束中的方法沒有充分利用預(yù)測的時(shí)空不確定性，這可能會(huì)導(dǎo)致次優(yōu)和不光滑的問題。
技術(shù)實(shí)現(xiàn)思路
1、針對(duì)現(xiàn)有技術(shù)中所存在的問題，本專利技術(shù)提供了一種動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法和系統(tǒng)，以解決現(xiàn)有技術(shù)中當(dāng)前簡單的預(yù)測動(dòng)態(tài)障礙物運(yùn)動(dòng)并將其加入軌跡規(guī)劃約束中的方法沒有充分利用預(yù)測的時(shí)空不確定性，這可能會(huì)導(dǎo)致次優(yōu)和不光滑的技術(shù)問題。
2、本專利技術(shù)提供了一種動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，包括：
3、s1、從獲取的點(diǎn)云數(shù)據(jù)中分割出靜態(tài)障礙物點(diǎn)云和動(dòng)態(tài)障礙物點(diǎn)云；
4、s2、對(duì)所述動(dòng)態(tài)障礙物進(jìn)行自適應(yīng)狀態(tài)估計(jì)；
5、s3、構(gòu)建所述靜態(tài)障礙物的概率柵格圖，并獲得所述靜態(tài)障礙物的概率柵格圖中柵格的占用概率信息；
6、s4、構(gòu)建所述動(dòng)態(tài)障礙物的概率柵格圖，并獲得所述動(dòng)態(tài)障礙物的概率柵格圖中柵格的占用概率信息；
7、s5、融合所述靜態(tài)障礙物的占用概率信息和動(dòng)態(tài)障礙物的占用概率信息，構(gòu)建時(shí)空概率占用地圖，并將無人機(jī)帶入所述時(shí)空概率占用地圖得到時(shí)空概率碰撞地圖；
8、s6、基于所述時(shí)空概率碰撞地圖規(guī)劃無人機(jī)的安全軌跡。
9、可選地，所述從獲取的點(diǎn)云數(shù)據(jù)中分割出靜態(tài)障礙物點(diǎn)云和動(dòng)態(tài)障礙物點(diǎn)云，包括：
10、針對(duì)點(diǎn)云數(shù)據(jù)中在tk時(shí)刻對(duì)應(yīng)的每一幀點(diǎn)云數(shù)據(jù)剔除所述每一幀點(diǎn)云數(shù)據(jù)中靜態(tài)障礙物點(diǎn)云的地面點(diǎn)，并采用dbscan算法將每一幀剔除地面點(diǎn)后的點(diǎn)云數(shù)據(jù)分割成一系列的點(diǎn)云簇，表示為：
11、
12、其中，表示第k幀點(diǎn)云中第l個(gè)點(diǎn)云簇，lk表示第k幀點(diǎn)云聚類出的點(diǎn)云簇的數(shù)量，并采用體素網(wǎng)格法計(jì)算所述點(diǎn)云簇的體積。
13、可選地，所述動(dòng)態(tài)障礙物進(jìn)行自適應(yīng)狀態(tài)估計(jì)，包括：
14、定義動(dòng)態(tài)障礙物n在tk時(shí)刻的狀態(tài)向量為并將動(dòng)態(tài)障礙物的動(dòng)態(tài)模型表示為：
15、
16、
17、其中，h為觀測矩陣，a是狀態(tài)轉(zhuǎn)移矩陣，z是動(dòng)態(tài)障礙物的位置，分別為過程高斯噪聲和觀測高斯噪聲；
18、并采用卡爾曼濾波預(yù)測過程高斯噪聲和采用實(shí)際新息序列來更新協(xié)方差矩陣，其中卡爾曼濾波的預(yù)測階段表示為：
19、
20、
21、其中，所述卡爾曼濾波的更新階段表示為：
22、
23、
24、
25、
26、其中，和分別為tk+1時(shí)刻的先驗(yàn)估計(jì)和后驗(yàn)估計(jì)，和分別為對(duì)應(yīng)的先驗(yàn)估計(jì)和后驗(yàn)估計(jì)的協(xié)方差矩陣，為卡爾曼增益，β∈(0,1)為遺忘因子，為過程高斯噪聲誤差的統(tǒng)計(jì)協(xié)方差，為最終過程噪聲協(xié)方差。
27、可選地，所述靜態(tài)障礙物的概率柵格圖，包括：
28、所述靜態(tài)障礙物的概率柵格圖表示為：
29、
30、其中，所述靜態(tài)障礙物的概率柵格圖中避障空間的下邊界為[xl,yl,zl]和上邊界為[xu,yu,zu]，所述概率柵格圖的每個(gè)柵格的大小為dm，則所述概率柵格圖中x,y,z維度的柵格數(shù)分別為i,j,k分別為x,y,z維度的索引，表示第(i,j,k)個(gè)柵格被占用的概率。
31、可選地，所述并獲得所述靜態(tài)障礙物的概率柵格圖中柵格的占用概率信息，包括：
32、針對(duì)每一幀點(diǎn)云數(shù)據(jù)中，所述點(diǎn)云簇中的每個(gè)點(diǎn)的所在柵格的索引表示為：
33、
34、
35、
36、其中，px,py,pz為點(diǎn)p的x,y,z坐標(biāo)，ip,jp,kp為點(diǎn)p所在的柵格的索引，并計(jì)算每個(gè)柵格被占用概率信息：
37、
38、其中，和分別是前個(gè)ks時(shí)刻的靜態(tài)障礙物在格柵中出現(xiàn)的次數(shù)和未出現(xiàn)的次數(shù)，所述ks＝5。
39、可選地，所述構(gòu)建動(dòng)態(tài)障礙物的概率柵格圖，包括：
40、在時(shí)域[tk,tk+h-1]內(nèi)的每個(gè)tk+h的時(shí)刻，每個(gè)動(dòng)態(tài)障礙物n都對(duì)應(yīng)一個(gè)動(dòng)態(tài)障礙物的概率柵格圖，所述動(dòng)態(tài)障礙物的概率柵格圖為：
41、
42、其中h＝0,1,…,h-1。
43、可選地，所述并獲得所述動(dòng)態(tài)障礙物的概率柵格圖中柵格的占用概率信息，包括：
44、計(jì)算動(dòng)態(tài)障礙物n在時(shí)刻索引是(i,j,k)的柵格被障礙物n占用的概率表示為：
45、
46、其中，s(i,j,k)為索引是(i,j,k)的柵格所占的空間，為動(dòng)態(tài)障礙物n的概率密度函數(shù)，采用數(shù)值積分對(duì)概率進(jìn)行近似求解，并表示為：
47、...
【技術(shù)保護(hù)點(diǎn)】
1.一種動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，包括：
2.如權(quán)利要求1所述的動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，所述從獲取的點(diǎn)云數(shù)據(jù)中分割出靜態(tài)障礙物點(diǎn)云和動(dòng)態(tài)障礙物點(diǎn)云，包括：
3.如權(quán)利要求2所述的動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，所述動(dòng)態(tài)障礙物進(jìn)行自適應(yīng)狀態(tài)估計(jì)，包括：
4.如權(quán)利要求3所述的動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，所述靜態(tài)障礙物的概率柵格圖，包括：
5.如權(quán)利要求4的動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，所述并獲得所述靜態(tài)障礙物的概率柵格圖中柵格的占用概率信息，包括：
6.如權(quán)利要求5的動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，所述構(gòu)建動(dòng)態(tài)障礙物的概率柵格圖，包括：
7.如權(quán)利要求6的動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，所述并獲得所述動(dòng)態(tài)障礙物的概率柵格圖中柵格的占用概率信息，包括：
8.如權(quán)利要求7的動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，所述融合所述靜態(tài)障礙物
9.如權(quán)利要求8的動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，所述基于所述時(shí)空概率碰撞地圖規(guī)劃無人機(jī)的安全軌跡，包括：
10.一種動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃系統(tǒng)，其特征在于，包括：
...【技術(shù)特征摘要】
1.一種動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，包括：
2.如權(quán)利要求1所述的動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，所述從獲取的點(diǎn)云數(shù)據(jù)中分割出靜態(tài)障礙物點(diǎn)云和動(dòng)態(tài)障礙物點(diǎn)云，包括：
3.如權(quán)利要求2所述的動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，所述動(dòng)態(tài)障礙物進(jìn)行自適應(yīng)狀態(tài)估計(jì)，包括：
4.如權(quán)利要求3所述的動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，所述靜態(tài)障礙物的概率柵格圖，包括：
5.如權(quán)利要求4的動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的安全軌跡規(guī)劃方法，其特征在于，所述并獲得所述靜態(tài)障礙物的概率柵格圖中柵格的占用概率信息，包括：
6.如權(quán)利要求5的動(dòng)態(tài)環(huán)境時(shí)空概率預(yù)測的...
【專利技術(shù)屬性】
技術(shù)研發(fā)人員：王鋼，周子煜，孫健，劉道明，肖偉，王際開，儲(chǔ)心懌，趙新煒，鄧方，
申請(qǐng)(專利權(quán))人：北京理工大學(xué)重慶創(chuàng)新中心，
類型：發(fā)明
國別省市：
還沒有人留言評(píng)論。發(fā)表了對(duì)其他瀏覽者有用的留言會(huì)獲得科技券。