一種基于自適應(yīng)ekf的無人偵察機(jī)同步定位與構(gòu)圖方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于無人機(jī)技術(shù)領(lǐng)域，具體涉及一種基于自適應(yīng)EKF的無人偵察機(jī)同步定 位與構(gòu)圖方法。
【背景技術(shù)】
[0002] 現(xiàn)代戰(zhàn)爭中，戰(zhàn)場環(huán)境復(fù)雜，及時(shí)掌握戰(zhàn)場情況對(duì)取得戰(zhàn)爭勝利起著決定性的作 用。無人偵察機(jī)應(yīng)運(yùn)而生，其能夠潛入敵區(qū)上空進(jìn)行偵查，為作戰(zhàn)指揮中心提供實(shí)時(shí)的敵區(qū) 圖像和信息，使得指揮官及時(shí)掌握敵軍情況，進(jìn)而制定合理的作戰(zhàn)計(jì)劃。其中，小型無人偵 查機(jī)以其體積小、重量輕、造價(jià)低、無傷亡等優(yōu)勢(shì)已經(jīng)成為目前研究的熱點(diǎn)。無人偵察機(jī)在 執(zhí)行任務(wù)過程中要求其具有續(xù)航時(shí)間長、導(dǎo)航精度高、不易被敵方發(fā)現(xiàn)等特性。其中高精度 的導(dǎo)航是無人偵察機(jī)成功完成任何任務(wù)的基本前提，因而恰當(dāng)?shù)膶?dǎo)航方法是無人偵察機(jī)完 成任務(wù)成敗的關(guān)鍵所在。
[0003] 導(dǎo)航是指無人偵察機(jī)機(jī)借助外部信號(hào)或通過自身攜帶的傳感器判斷自身位置，從 而指導(dǎo)其航行、完成預(yù)定任務(wù)的過程。目前無人偵察機(jī)的導(dǎo)航方法主要分為兩種，非自主導(dǎo) 航以及自主導(dǎo)航。
[0004] 非自主導(dǎo)航方法主要是衛(wèi)星導(dǎo)航，利用GPS、歐米茄、羅蘭、北斗等，能夠獲得位置、 速度、時(shí)間等導(dǎo)航信息，且精度不隨時(shí)間推移而降低。然而GPS無法提供無人偵察機(jī)的姿態(tài) 信息，此外，目前只有GPS是覆蓋全球的，這就使得無人偵察機(jī)在執(zhí)行大范圍任務(wù)時(shí)難免受 制于人，并且應(yīng)用環(huán)境的適應(yīng)性較差，容易受到干擾。自主導(dǎo)航不依賴外界信號(hào)源，僅僅通 過自身攜帶的速度、姿態(tài)及環(huán)境感知傳感器即可實(shí)現(xiàn)相對(duì)準(zhǔn)確的導(dǎo)航，但是對(duì)于導(dǎo)航算法 的要求較高，是目前無人偵察機(jī)導(dǎo)航領(lǐng)域的研究重點(diǎn)及難點(diǎn)。
[0005] 自主導(dǎo)航方法主要有航位推算、視覺導(dǎo)航、地球物理導(dǎo)航、多普勒導(dǎo)航、慣性導(dǎo)航、 同步定位與構(gòu)圖等。航位推算具有較強(qiáng)的自主性、保密性及抗干擾性，但是其推算誤差隨著 時(shí)間迅速積累，不適合長航時(shí)無人偵察機(jī)導(dǎo)航；視覺導(dǎo)航具有獨(dú)立性、準(zhǔn)確性，但是只能獲 得相對(duì)的運(yùn)動(dòng)狀態(tài)信息；地球物理導(dǎo)航具有無源性、無輻射、隱蔽性強(qiáng)，但是需要大量的地 磁數(shù)據(jù)，實(shí)時(shí)性受計(jì)算機(jī)處理能力限制，且受地形，天氣影響；多普勒導(dǎo)航自主性好、測(cè)速精 度高、抗干擾性強(qiáng)，但是其工作時(shí)必須發(fā)射電波，隱蔽性不好；慣性導(dǎo)航，精度高、隱蔽性好， 短期內(nèi)具有很高的導(dǎo)航定位精度，但慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)較為困難，且造價(jià)比較高。
[0006] 同步定位與地圖構(gòu)建（SimultaneousLocalizationandMapping，SLAM)技術(shù)的 提出，一定程度上緩解了上述導(dǎo)航方法中存在的不足。SLAM是在無人偵察機(jī)自身位置不確 定，航位推算的過程中位置誤差逐漸累積的情況下，利用自身攜帶的環(huán)境感知傳感器反復(fù) 探測(cè)環(huán)境中特征，從而完成自身及特征位置的校正，同時(shí)構(gòu)建環(huán)境地圖，無需預(yù)知地形信息 或外部輔助定位設(shè)備即可以得到較為可靠的無人偵察機(jī)位置信息及環(huán)境地圖信息，詳細(xì)描 述如圖1所示。

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

[0007] (一）要解決的技術(shù)問題
[0008] 本發(fā)明要解決的技術(shù)問題是：如何將自適應(yīng)濾波方法與SLAM方法相結(jié)合以解決 無人偵查機(jī)自主導(dǎo)航過程中存在的位置誤差累計(jì)、環(huán)境適應(yīng)性差、初始對(duì)準(zhǔn)困難及隱蔽性 差等問題。
[0009] (二）技術(shù)方案
[0010] 為解決上述技術(shù)問題，本發(fā)明提供一種基于自適應(yīng)EKF的無人偵察機(jī)同步定位與 構(gòu)圖方法，該方法包括如下步驟：
[0011] 步驟S1 :對(duì)無人偵察機(jī)SLAM系統(tǒng)建模；
[0012] 1.1地圖模型
[0013] 采用特征地圖來構(gòu)建環(huán)境地圖模型，將環(huán)境中的目標(biāo)用幾何原型來描述；
[0014] 1.2坐標(biāo)系統(tǒng)
[0015] 由于無人偵察機(jī)所攜帶的傳感器與環(huán)境特征之間的感知行為發(fā)生在傳感器坐標(biāo) 系中，速度的測(cè)量發(fā)生在無人偵察機(jī)運(yùn)動(dòng)坐標(biāo)系中，而最終環(huán)境地圖的構(gòu)建需要表示在全 局地圖中，所以建立全局坐標(biāo)系，無人偵察機(jī)運(yùn)動(dòng)坐標(biāo)系及傳感器坐標(biāo)系，并表明他們之間 的關(guān)系，定義全局坐標(biāo)系，G為地心，X指向正北，Y指向正東，Z垂直于X平面 并指向G;定義AXaYaZa為無人偵察機(jī)運(yùn)動(dòng)坐標(biāo)系，A為無人偵察機(jī)質(zhì)心，XA指向機(jī)頭， 直于機(jī)頭，ΖΑ垂直于ΧΑΥΑ平面并指向A;定義SXsYsZs為環(huán)境感知傳感器坐標(biāo)系，S為傳感器 質(zhì)心，Xs指向傳感器電磁波發(fā)射方向，Ys垂直于電磁波發(fā)射方向，Zs垂直于XSYS平面并指向 S;三個(gè)坐標(biāo)系均符合右手定則；
[0016] 1.3特征模型
[0017] 根據(jù)公式（1)，定義特征的運(yùn)動(dòng)學(xué)模型為：
[0018] Xfi (k) =Xfi (k~l)i= 1···η (1)
[0019] 其中，k為離散時(shí)間變量；
[0020] 特征在傳感器坐標(biāo)系中一般以極坐標(biāo)的形式表示：
[0021]
[0022] 其中，Xfl表示第i個(gè)特征的狀態(tài)，即位置；η為特征的個(gè)數(shù)；
[0023] Pi為特征i在極坐標(biāo)系下的極徑；Θi為特征i在極坐標(biāo)系下的極角；
[0024] 1. 4無人偵察機(jī)運(yùn)動(dòng)模型
[0025] 通過建立一個(gè)四自由度的常速運(yùn)動(dòng)學(xué)模型來描述無人偵察機(jī)的運(yùn)動(dòng)變換趨勢(shì)，如 公式（2)所示：
[0026] XA (k) =f(XA (k-1),n(k~l)) (2)
[0027] 無人偵察機(jī)XA=[xyzΦuvwr]T，[x,y,z,Φ]為全局坐標(biāo)系中無人偵察機(jī) 的位置和艏向，[u，v，w，r]表示運(yùn)動(dòng)坐標(biāo)系A(chǔ)中相應(yīng)的XA、YA、ZA方向的線速度和轉(zhuǎn)艏角速 度，n= [nu,nv,nw,nj表示以加速度的形式作用在速度上的高斯噪聲；
[0028] 根據(jù)特征狀態(tài)Xfl和無人偵察機(jī)狀態(tài)X&可以獲得SLAM系統(tǒng)的狀態(tài)X，包括無人偵 察機(jī)的狀態(tài)XA和特征的狀態(tài)Xf:
[0029] X=[XAXf]T ⑶
[0030]
[0031] 其中，T為采樣時(shí)間；
[0032] 1. 5傳感器測(cè)量模型
[0033] (1)速度測(cè)量模型
[0034] 速度測(cè)量傳感器可提供X、Υ、Ζ三個(gè)方向的速度，測(cè)量模型為：
[0035] Zv=HvX+sv (4)
[0036] 其中，Zv為速度的測(cè)量值；
[0037] 測(cè)量矩陣比為：
[0038] Ηγ -[03X4Ι3Χ303Χ103Χ2η]
[0039] 其中，0為全零矩陣，I為單位矩陣；
[0040] 速度測(cè)量噪聲sv的協(xié)方差矩陣為：
[0041]
[0042] 其中，cjI為速度u的方差，4，為速度v的方差，為速度w的方差；
[0043] (2)高度測(cè)量模型
[0044] 通過高度測(cè)量傳感器可以獲得無人偵察機(jī)的高度，測(cè)量模型為：
[0045] Zh=HhX+sh (5)
[0046] 其中，ZH為高度的測(cè)量值；
[0047] 測(cè)量矩陣Hh為：
[0048] Hh= [0 0 1 0 0 0 0 0 01X2n]
[0049] 高度測(cè)量噪聲的sH協(xié)方差矩陣表示為：
[0050]
[0051] ⑶艏向測(cè)量模型
[0052] 通過艏向測(cè)量傳感器可以獲得無人偵察機(jī)的艏向，測(cè)量模型為：
[0053] Zc=HcX+sc (6)
[0054] 其中，Zc為艏向的測(cè)量值；
[0055] 測(cè)量矩陣氏為：
[0056] Hc= [0 00100000 1X2n]
[0057] 艏向測(cè)量噪聲sc的協(xié)方差為：
[0058] Rc 二
[0059] (4)環(huán)境測(cè)量模型
[0060] 通過環(huán)境感知傳感器可以獲得環(huán)境中特征相對(duì)于無人偵查機(jī)的距離和方位，測(cè)量 模型為：
[0061]
：(J);
[0062] 其中，Sl為特征測(cè)量噪聲；
[0063]
[0064] 測(cè)量矩陣為：
[0065]
[0066] 其中，/^和貧4是點(diǎn)特征在運(yùn)動(dòng)坐標(biāo)系下的參數(shù)表示，pJPΘ1是點(diǎn)特征在全局 坐標(biāo)系下的參數(shù)表示；
[0067