一種基于多模型分布式濾波的無人機(jī)自主導(dǎo)航與定位方法
【專利摘要】本發(fā)明公開了一種基于多模型分布式濾波的無人機(jī)自主導(dǎo)航與定位方法，目的是不僅要降低無人機(jī)自主導(dǎo)航與定位方法的計(jì)算復(fù)雜度，滿足嵌入式處理器實(shí)時(shí)處理的要求，還要保證對(duì)無人機(jī)進(jìn)行穩(wěn)定控制，而且在衛(wèi)星導(dǎo)航定位系統(tǒng)短期或長(zhǎng)期不可用時(shí)，也能保證無人機(jī)實(shí)現(xiàn)自主導(dǎo)航和定位。該方法通過建立不同階次的多模型分布式濾波狀態(tài)估計(jì)的系統(tǒng)狀態(tài)方程，利用不同量測(cè)系統(tǒng)和傳感器獲得無人機(jī)運(yùn)動(dòng)的量測(cè)值，采用分布式濾波方法來估計(jì)并補(bǔ)償?shù)统杀緫T性導(dǎo)航系統(tǒng)的誤差，為無人機(jī)提供連續(xù)、可靠的導(dǎo)航和定位信息。本發(fā)明能夠長(zhǎng)時(shí)間連續(xù)、穩(wěn)定地為無人機(jī)控制系統(tǒng)提供精確的導(dǎo)航和定位信息。
【專利說明】一種基于多模型分布式濾波的無人機(jī)自主導(dǎo)航與定位方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種無人機(jī)自主導(dǎo)航和定位方法，特別是一種基于多模型分布式濾波的無人機(jī)自主導(dǎo)航與定位方法，適用于無人飛行器和有人飛行器自主導(dǎo)航和定位系統(tǒng)。
【背景技術(shù)】
[0002]與有人機(jī)相比，無人機(jī)是低風(fēng)險(xiǎn)和低成本，而且在軍民用領(lǐng)域中被廣泛應(yīng)用，例如空中監(jiān)視、地面?zhèn)刹臁?zāi)后重建、遙感探測(cè)以及地面目標(biāo)跟蹤等。在實(shí)際應(yīng)用中，無人機(jī)對(duì)自主導(dǎo)航系統(tǒng)精度和可靠性要求越來越高，要求實(shí)時(shí)、可靠地提供無人飛行器的位置、速度和姿態(tài)信息。由于低成本慣性導(dǎo)航系統(tǒng)誤差發(fā)散很快，依靠單一的低成本慣性導(dǎo)航系統(tǒng)很難滿足要求。將慣性導(dǎo)航系統(tǒng)與衛(wèi)星定位系統(tǒng)信息進(jìn)行融合來提供載體的運(yùn)動(dòng)信息已被廣泛應(yīng)用。但由于衛(wèi)星定位信號(hào)易受干擾或欺騙，甚至信號(hào)轉(zhuǎn)發(fā)源被摧毀，不能夠長(zhǎng)期提供穩(wěn)定可靠的定位信息，在軍民用領(lǐng)域中尚不能完全依賴于衛(wèi)星定位來輔助慣性導(dǎo)航實(shí)現(xiàn)飛行器的精確導(dǎo)航定位。因此，發(fā)展一些替代的導(dǎo)航系統(tǒng)用于處理衛(wèi)星導(dǎo)航系統(tǒng)短期和長(zhǎng)期故障時(shí)的無人機(jī)自主導(dǎo)航和定位問題，已成為軍民用領(lǐng)域無人機(jī)應(yīng)用研究的的重點(diǎn)。視覺相機(jī)作為無人機(jī)配重包中的標(biāo)準(zhǔn)傳感器既可用于導(dǎo)航傳感器獲取無人機(jī)的導(dǎo)航和定位信息，又可以用于任務(wù)傳感器執(zhí)行特定的任務(wù)，例如偵查，運(yùn)動(dòng)目標(biāo)跟蹤以及遙感探測(cè)，已成為無人機(jī)自主導(dǎo)航和定位的有效補(bǔ)充導(dǎo)航源之一。
[0003]根據(jù)無人機(jī)飛行的環(huán)境是否已知將基于視覺信息的導(dǎo)航和定位分為兩類，一類是飛行環(huán)境未知，常用的方法有同時(shí)完成定位和生成環(huán)境地圖的SLAM算法和視覺測(cè)距方法，前者在室內(nèi)無人機(jī)和機(jī)器人導(dǎo)航定位中已廣泛應(yīng)用，但在室外環(huán)境多變的大視場(chǎng)環(huán)境無法有效使用；雖然后者可以用于室外多變環(huán)境，但需要精確獲取無人機(jī)的飛行姿態(tài)和穩(wěn)定平臺(tái)的姿態(tài)；另一類是飛行環(huán)境已知，常用方法有陸標(biāo)匹配和圖像匹配定位，可以給出無人機(jī)精確的導(dǎo)航和位置信息。
[0004]本發(fā)明為中提到的一種基于多模型分布式濾波的無人機(jī)自主導(dǎo)航與定位方法，該方法在東北天地里坐標(biāo)系下，根據(jù)慣性導(dǎo)航系統(tǒng)力學(xué)方程編排以及慣性導(dǎo)航系統(tǒng)誤差方程建立不同階次的多模型分布式濾波狀態(tài)估計(jì)的系統(tǒng)狀態(tài)方程，利用不同量測(cè)系統(tǒng)和傳感器獲得無人機(jī)運(yùn)動(dòng)的量測(cè)值，采用分布式濾波方法來估計(jì)并補(bǔ)償?shù)统杀緫T性導(dǎo)航系統(tǒng)的誤差，為無人機(jī)提供連續(xù)、可靠的導(dǎo)航和定位信息。
[0005]本發(fā)明與其它無人機(jī)導(dǎo)航和定位方法不同在于:在當(dāng)前已公開發(fā)表的文獻(xiàn)中，依靠慣性導(dǎo)航/GPS組合導(dǎo)航或者是依靠視覺輔助慣性導(dǎo)航來提供無人機(jī)的導(dǎo)航和定位信息，但這些導(dǎo)航方法在實(shí)際應(yīng)用中依然存在計(jì)算復(fù)雜度高以及在一些環(huán)境中無法使用的局限性，特別是在衛(wèi)星導(dǎo)航定位系統(tǒng)信號(hào)長(zhǎng)時(shí)間不可用時(shí)，導(dǎo)航系統(tǒng)無法提供長(zhǎng)時(shí)間穩(wěn)定的姿態(tài)信息而導(dǎo)致無人機(jī)及其機(jī)載設(shè)備損毀的風(fēng)險(xiǎn)。

【發(fā)明內(nèi)容】

[0006]本發(fā)明要解決的技術(shù)問題:為解決在衛(wèi)星導(dǎo)航定位系統(tǒng)長(zhǎng)時(shí)間不可用時(shí)，導(dǎo)航系統(tǒng)能夠長(zhǎng)時(shí)間提供穩(wěn)定的姿態(tài)和定位信息來控制無人機(jī)穩(wěn)定、可靠地飛行，提出一種基于多模型分布式濾波的無人機(jī)自主導(dǎo)航與定位方法。
[0007]本發(fā)明采用的技術(shù)方案為:一種基于多模型分布式濾波的無人機(jī)自主導(dǎo)航與定位方法，該方法在東北天地里坐標(biāo)系下，根據(jù)慣性導(dǎo)航系統(tǒng)力學(xué)方程編排以及慣性導(dǎo)航系統(tǒng)誤差方程建立不同階次的多模型分布式濾波系統(tǒng)狀態(tài)方程，利用不同量測(cè)系統(tǒng)和傳感器獲得無人機(jī)運(yùn)動(dòng)的量測(cè)值，采用分布式濾波方法來估計(jì)并補(bǔ)償?shù)统杀緫T性導(dǎo)航系統(tǒng)的誤差，為無人機(jī)提供連續(xù)、可靠的導(dǎo)航和定位信息。保證在衛(wèi)星導(dǎo)航定位系統(tǒng)短期或長(zhǎng)期不可用時(shí)，能夠長(zhǎng)時(shí)間連續(xù)、穩(wěn)定地為無人機(jī)提供精確的導(dǎo)航和定位信息。具體實(shí)現(xiàn)步驟為:
[0008]步驟(I )、當(dāng)衛(wèi)星導(dǎo)航系統(tǒng)可用時(shí)，采用七狀態(tài)反饋Kalman濾波處理衛(wèi)星導(dǎo)航定位系統(tǒng)輸出的位置和速度信息來估計(jì)慣性導(dǎo)航系統(tǒng)水平通道和姿態(tài)誤差，使慣性導(dǎo)航系統(tǒng)輸出精確的位置、速度和姿態(tài)信息，其具體方法為:
[0009]al、根據(jù)慣性導(dǎo)航系統(tǒng)力學(xué)方程編排，利用加速度計(jì)和陀螺儀的輸出值實(shí)時(shí)計(jì)算無人機(jī)的運(yùn)動(dòng)信息，包括位置、速度和姿態(tài)；
[0010]a2、根據(jù)慣性導(dǎo)航系統(tǒng)誤差方程建立慣性導(dǎo)航/衛(wèi)星導(dǎo)航組合導(dǎo)航系統(tǒng)狀態(tài)方程為:
[0011]
【權(quán)利要求】
1.一種基于多模型分布式濾波的無人機(jī)自主導(dǎo)航與定位方法，其特征在于:該方法在東北天地里坐標(biāo)系下，根據(jù)慣性導(dǎo)航系統(tǒng)力學(xué)方程編排以及慣性導(dǎo)航系統(tǒng)誤差方程建立不同階次的多模型分布式濾波狀態(tài)估計(jì)的系統(tǒng)狀態(tài)方程，利用不同量測(cè)系統(tǒng)和傳感器獲得無人機(jī)運(yùn)動(dòng)的量測(cè)值，采用分布式濾波方法來估計(jì)并補(bǔ)償?shù)统杀緫T性導(dǎo)航系統(tǒng)的誤差，其具體實(shí)現(xiàn)過程如下: 步驟(I )、當(dāng)衛(wèi)星導(dǎo)航系統(tǒng)可用時(shí)，采用七狀態(tài)反饋Kalman濾波處理衛(wèi)星導(dǎo)航定位系統(tǒng)輸出的位置和速度信息來估計(jì)慣性導(dǎo)航系統(tǒng)水平通道和姿態(tài)誤差，使慣性導(dǎo)航系統(tǒng)輸出精確的位置、速度和姿態(tài)信息； 步驟(2)、采用四狀態(tài)反饋Kalman濾波處理氣壓計(jì)或衛(wèi)星導(dǎo)航定位系統(tǒng)輸出的高度來估計(jì)并補(bǔ)償慣性導(dǎo)航系統(tǒng)垂直通道的誤差，獲得慣性導(dǎo)航系統(tǒng)輸出精確的高度和垂直通道速度信息； 步驟(3)、當(dāng)衛(wèi)星導(dǎo)航定位系統(tǒng)不可用時(shí)，采用四狀態(tài)擴(kuò)展Kalman濾波估計(jì)并補(bǔ)償四元數(shù)誤差，保證慣性導(dǎo)航系統(tǒng)長(zhǎng)時(shí)間輸出穩(wěn)定的姿態(tài)信息； 步驟(4)、當(dāng)衛(wèi)星導(dǎo)航定位系統(tǒng)不可用時(shí)，采用不等間隔Kalman濾波處理視覺傳感器提供的量測(cè)信息來估計(jì)并補(bǔ)償慣性導(dǎo)航系統(tǒng)的位置和速度誤差；保證慣性導(dǎo)航系統(tǒng)輸出穩(wěn)定的位置和速度信息。
2.根據(jù)權(quán)利要求1所述的一種基于多模型分布式濾波的無人機(jī)自主導(dǎo)航與定位方法，其特征在于:所述步驟(1)當(dāng)衛(wèi)星導(dǎo)航系統(tǒng)可用時(shí)，采用七狀態(tài)反饋Kalman濾波處理衛(wèi)星導(dǎo)航定位系統(tǒng)輸出的位置和速度信息來估計(jì)慣性導(dǎo)航系統(tǒng)水平通道和姿態(tài)誤差，使慣性導(dǎo)航系統(tǒng)輸出精確的位置、速度和姿態(tài)信息，其具體方法為: al、根據(jù)慣性導(dǎo)航系統(tǒng)力學(xué)方程編排，利用加速度計(jì)和陀螺儀的輸出值實(shí)時(shí)計(jì)算無人機(jī)的運(yùn)動(dòng)信息，包括位置、速度和姿態(tài)； a2、根據(jù)慣性導(dǎo)航系統(tǒng)誤差方程建立慣性導(dǎo)航/衛(wèi)星導(dǎo)航組合導(dǎo)航系統(tǒng)狀態(tài)方程為:
3.根據(jù)權(quán)利要求1所述的一種基于多模型分布式濾波的無人機(jī)自主導(dǎo)航與定位方法，其特征在于:所述步驟(2)采用四狀態(tài)反饋Kalman濾波處理氣壓計(jì)或衛(wèi)星導(dǎo)航定位系統(tǒng)輸出的高度來估計(jì)并補(bǔ)償慣性導(dǎo)航系統(tǒng)垂直通道的誤差，獲得慣性導(dǎo)航系統(tǒng)輸出精確的高度和垂直通道速度信息，其具體方法為: bl、根據(jù)慣性導(dǎo)航系統(tǒng)力學(xué)方程編排，利用加速度計(jì)和陀螺儀輸出信息計(jì)算無人機(jī)的高度和垂向速度； b2、根據(jù)慣性導(dǎo)航垂直通道的力學(xué)方程編排以及氣壓計(jì)誤差模型建立垂直通道誤差估計(jì)的系統(tǒng)狀態(tài)方程為: Xk= Oklk-A-A-1 ⑷
4.根據(jù)權(quán)利要求1所述的一種基于多模型分布式濾波的無人機(jī)自主導(dǎo)航與定位方法，其特征在于:所述步驟(3)當(dāng)衛(wèi)星導(dǎo)航定位系統(tǒng)不可用時(shí)，采用四狀態(tài)擴(kuò)展Kalman濾波估計(jì)并補(bǔ)償四元數(shù)誤差，保證慣性導(dǎo)航系統(tǒng)長(zhǎng)時(shí)間輸出穩(wěn)定的姿態(tài)信息，其具體方法為: Cl、利用加速度計(jì)、陀螺儀和磁傳感器輸出的信息計(jì)算無人機(jī)的姿態(tài)為:
5.根據(jù)權(quán)利要求2所述的一種基于多模型分布式濾波的無人機(jī)自主導(dǎo)航與定位方法，其特征在于:所述步驟(4)當(dāng)衛(wèi)星導(dǎo)航定位系統(tǒng)不可用時(shí)，采用不等間隔Kalman濾波處理視覺傳感器提供的量測(cè)信息來估計(jì)并補(bǔ)償慣性導(dǎo)航系統(tǒng)的位置和速度誤差；保證慣性導(dǎo)航系統(tǒng)輸出穩(wěn)定的位置和速度信息，其具體方法為: dl、當(dāng)無人機(jī)飛過區(qū)域的地物特征豐富且預(yù)裝了飛行區(qū)域的基準(zhǔn)圖像，采用基于灰度信息的景像匹配定位方法獲取無人機(jī)的位置，基于灰度信息的景像匹配方法有粗匹配和精匹配兩個(gè)過程，粗匹配采用交叉相關(guān)算法，精匹配采用最小二乘算法； d2、當(dāng)無人機(jī)飛過的區(qū)域地物特征不豐富或無人機(jī)未預(yù)裝飛行區(qū)域的基準(zhǔn)圖，采用加速魯棒特征SURF算法提取相鄰兩幀實(shí)時(shí)圖像的特征點(diǎn)并進(jìn)行同名特征點(diǎn)匹配，利用隨機(jī)抽樣一致性RANSAC算法剔除誤匹配點(diǎn)，并根據(jù)相鄰兩幀的同名特征點(diǎn)計(jì)算無人機(jī)位置信息為:
【文檔編號(hào)】G01C21/16GK103697889SQ201310741915
【公開日】2014年4月2日 申請(qǐng)日期:2013年12月29日 優(yōu)先權(quán)日:2013年12月29日
【發(fā)明者】趙龍, 高楠, 閆泓宇, 王丁 申請(qǐng)人:北京航空航天大學(xué)