本發(fā)明專利技術(shù)屬于無人機技術(shù)領(lǐng)域，具體涉及一種基于自適應(yīng)EKF的無人偵察機同步定位與構(gòu)圖方法。其建立無人偵察機同步定位與地圖構(gòu)建系統(tǒng)的地圖模型、坐標系統(tǒng)、特征模型、無人偵察機的運動模型以及傳感器測量模型。利用EKF實現(xiàn)無人偵察機的同步定位與地圖構(gòu)建方法框架的設(shè)計。并針對無人偵察機運動模型、觀測模型構(gòu)建不準確、以及非線性模型線性化導(dǎo)致的模型不匹配問題研究。提出將模型構(gòu)建不準確以及線性化誤差導(dǎo)致的不確定性建模為具有時變均值及協(xié)方差矩陣的系統(tǒng)過程噪聲和觀測噪聲，設(shè)計基于帶時變噪聲估值器的自適應(yīng)EKF的同步定位與構(gòu)圖方法，實時估計系統(tǒng)過程噪聲和觀測的統(tǒng)計特性并對其進行修正，從而降低模型不確定性，提高估計進度。
【技術(shù)實現(xiàn)步驟摘要】
本專利技術(shù)屬于無人機
，具體涉及一種基于自適應(yīng)EKF的無人偵察機同步定 位與構(gòu)圖方法。
技術(shù)介紹
現(xiàn)代戰(zhàn)爭中，戰(zhàn)場環(huán)境復(fù)雜，及時掌握戰(zhàn)場情況對取得戰(zhàn)爭勝利起著決定性的作 用。無人偵察機應(yīng)運而生，其能夠潛入敵區(qū)上空進行偵查，為作戰(zhàn)指揮中心提供實時的敵區(qū) 圖像和信息，使得指揮官及時掌握敵軍情況，進而制定合理的作戰(zhàn)計劃。其中，小型無人偵 查機以其體積小、重量輕、造價低、無傷亡等優(yōu)勢已經(jīng)成為目前研究的熱點。無人偵察機在 執(zhí)行任務(wù)過程中要求其具有續(xù)航時間長、導(dǎo)航精度高、不易被敵方發(fā)現(xiàn)等特性。其中高精度 的導(dǎo)航是無人偵察機成功完成任何任務(wù)的基本前提，因而恰當(dāng)?shù)膶?dǎo)航方法是無人偵察機完 成任務(wù)成敗的關(guān)鍵所在。 導(dǎo)航是指無人偵察機機借助外部信號或通過自身攜帶的傳感器判斷自身位置，從 而指導(dǎo)其航行、完成預(yù)定任務(wù)的過程。目前無人偵察機的導(dǎo)航方法主要分為兩種，非自主導(dǎo) 航以及自主導(dǎo)航。 非自主導(dǎo)航方法主要是衛(wèi)星導(dǎo)航，利用GPS、歐米茄、羅蘭、北斗等，能夠獲得位置、 速度、時間等導(dǎo)航信息，且精度不隨時間推移而降低。然而GPS無法提供無人偵察機的姿態(tài) 信息，此外，目前只有GPS是覆蓋全球的，這就使得無人偵察機在執(zhí)行大范圍任務(wù)時難免受 制于人，并且應(yīng)用環(huán)境的適應(yīng)性較差，容易受到干擾。自主導(dǎo)航不依賴外界信號源，僅僅通 過自身攜帶的速度、姿態(tài)及環(huán)境感知傳感器即可實現(xiàn)相對準確的導(dǎo)航，但是對于導(dǎo)航算法 的要求較高，是目前無人偵察機導(dǎo)航領(lǐng)域的研究重點及難點。 自主導(dǎo)航方法主要有航位推算、視覺導(dǎo)航、地球物理導(dǎo)航、多普勒導(dǎo)航、慣性導(dǎo)航、 同步定位與構(gòu)圖等。航位推算具有較強的自主性、保密性及抗干擾性，但是其推算誤差隨著 時間迅速積累，不適合長航時無人偵察機導(dǎo)航；視覺導(dǎo)航具有獨立性、準確性，但是只能獲 得相對的運動狀態(tài)信息；地球物理導(dǎo)航具有無源性、無輻射、隱蔽性強，但是需要大量的地 磁數(shù)據(jù)，實時性受計算機處理能力限制，且受地形，天氣影響；多普勒導(dǎo)航自主性好、測速精 度高、抗干擾性強，但是其工作時必須發(fā)射電波，隱蔽性不好；慣性導(dǎo)航，精度高、隱蔽性好， 短期內(nèi)具有很高的導(dǎo)航定位精度，但慣導(dǎo)系統(tǒng)初始對準較為困難，且造價比較高。 同步定位與地圖構(gòu)建（SimultaneousLocalizationandMapping，SLAM)技術(shù)的 提出，一定程度上緩解了上述導(dǎo)航方法中存在的不足。SLAM是在無人偵察機自身位置不確 定，航位推算的過程中位置誤差逐漸累積的情況下，利用自身攜帶的環(huán)境感知傳感器反復(fù) 探測環(huán)境中特征，從而完成自身及特征位置的校正，同時構(gòu)建環(huán)境地圖，無需預(yù)知地形信息 或外部輔助定位設(shè)備即可以得到較為可靠的無人偵察機位置信息及環(huán)境地圖信息，詳細描 述如圖1所示。
技術(shù)實現(xiàn)思路
(一）要解決的技術(shù)問題 本專利技術(shù)要解決的技術(shù)問題是：如何將自適應(yīng)濾波方法與SLAM方法相結(jié)合以解決 無人偵查機自主導(dǎo)航過程中存在的位置誤差累計、環(huán)境適應(yīng)性差、初始對準困難及隱蔽性 差等問題。 (二）技術(shù)方案 為解決上述技術(shù)問題，本專利技術(shù)提供一種基于自適應(yīng)EKF的無人偵察機同步定位與 構(gòu)圖方法，該方法包括如下步驟： 步驟S1 :對無人偵察機SLAM系統(tǒng)建模； 1.1地圖模型 采用特征地圖來構(gòu)建環(huán)境地圖模型，將環(huán)境中的目標用幾何原型來描述； 1.2坐標系統(tǒng) 由于無人偵察機所攜帶的傳感器與環(huán)境特征之間的感知行為發(fā)生在傳感器坐標 系中，速度的測量發(fā)生在無人偵察機運動坐標系中，而最終環(huán)境地圖的構(gòu)建需要表示在全 局地圖中，所以建立全局坐標系，無人偵察機運動坐標系及傳感器坐標系，并表明他們之間 的關(guān)系，定義全局坐標系，G為地心，X指向正北，Y指向正東，Z垂直于X平面 并指向G;定義AXaYaZa為無人偵察機運動坐標系，A為無人偵察機質(zhì)心，XA指向機頭， 直于機頭，ΖΑ垂直于ΧΑΥΑ平面并指向A;定義SXsYsZs為環(huán)境感知傳感器坐標系，S為傳感器 質(zhì)心，Xs指向傳感器電磁波發(fā)射方向，Ys垂直于電磁波發(fā)射方向，Zs垂直于XSYS平面并指向 S;三個坐標系均符合右手定則； 1.3特征模型 根據(jù)公式（1)，定義特征的運動學(xué)模型為： Xfi (k) =Xfi (k~l)i= 1···η (1) 其中，k為離散時間變量； 特征在傳感器坐標系中一般以極坐標的形式表示： 其中，Xfl表示第i個特征的狀態(tài)，即位置；η為特征的個數(shù)； Pi為特征i在極坐標系下的極徑；Θi為特征i在極坐標系下的極角； 1. 4無人偵察機運動模型 通過建立一個四自由度的常速運動學(xué)模型來描述無人偵察機的運動變換趨勢，如 公式（2)所示： XA (k) =f(XA (k-1),n(k~l)) (2) 無人偵察機XA=T，為全局坐標系中無人偵察機 的位置和艏向，表示運動坐標系A(chǔ)中相應(yīng)的XA、YA、ZA方向的線速度和轉(zhuǎn)艏角速 度，n= [nu,nv,nw,nj表示以加速度的形式作用在速度上的高斯噪聲； 根據(jù)特征狀態(tài)Xfl和無人偵察機狀態(tài)X&可以獲得SLAM系統(tǒng)的狀態(tài)X，包括無人偵 察機的狀態(tài)XA和特征的狀態(tài)Xf: X=T ⑶ 其中，T為采樣時間； 1. 5傳感器測量模型 (1)速度測量模型 速度測量傳感器可提供X、Υ、Ζ三個方向的速度，測量模型為： Zv=HvX+sv (4) 其中，Zv為速度的測量值； 測量矩陣比為： Ηγ - 其中，0為全零矩陣，I為單位矩陣； 速度測量噪聲sv的協(xié)方差矩陣為： 其中，cjI為速度u的方差，4，為速度v的方差，為速度w的方差； (2)高度測量模型 通過高度測量傳感器可以獲得無人偵察機的高度，測量模型為： Zh=HhX+sh (5) 其中，ZH為高度的測量值； 測量矩陣Hh為： Hh= 高度測量噪聲的sH協(xié)方差矩陣表示為： ⑶艏向測量模型 通過艏向測量傳感器可以獲得無人偵察機的艏向，測量模型為： Zc=HcX+sc (6) 其中，Zc為艏向的測量值； 測量矩陣氏為： Hc= 艏向測量噪聲sc的協(xié)方差為： Rc 二 (4)環(huán)境測量模型 通過環(huán)境感知傳感器可以獲得環(huán)境中特征相對于無人偵查機的距離和方位，測量 模型為：：(J); 其中，Sl為特征測量噪聲； 測量矩陣為： 其中，/^和貧4是點特征在運動坐標系下的參數(shù)表示，pJPΘ1是點特征在全局 坐標系下的參數(shù)表示；[0067當(dāng)前第1頁1 2 3 4 本文檔來自技高網(wǎng)...
【技術(shù)保護點】
一種基于自適應(yīng)EKF的無人偵察機同步定位與構(gòu)圖方法，其特征在于，該方法包括如下步驟：步驟S1：對無人偵察機SLAM系統(tǒng)建模；1.1地圖模型采用特征地圖來構(gòu)建環(huán)境地圖模型，將環(huán)境中的目標用幾何原型來描述；1.2坐標系統(tǒng)由于無人偵察機所攜帶的傳感器與環(huán)境特征之間的感知行為發(fā)生在傳感器坐標系中，速度的測量發(fā)生在無人偵察機運動坐標系中，而最終環(huán)境地圖的構(gòu)建需要表示在全局地圖中，所以建立全局坐標系，無人偵察機運動坐標系及傳感器坐標系，并表明他們之間的關(guān)系，定義GXGYGZG為全局坐標系，G為地心，XG指向正北，YG指向正東，ZG垂直于XGYG平面并指向G；定義AXAYAZA為無人偵察機運動坐標系，A為無人偵察機質(zhì)心，XA指向機頭，YA垂直于機頭，ZA垂直于XAYA平面并指向A；定義SXSYSZS為環(huán)境感知傳感器坐標系，S為傳感器質(zhì)心，XS指向傳感器電磁波發(fā)射方向，YS垂直于電磁波發(fā)射方向，ZS垂直于XSYS平面并指向S；三個坐標系均符合右手定則；1.3特征模型根據(jù)公式(1)，定義特征的運動學(xué)模型為：Xfi(k)=Xfi(k-1),i=1...n---(1)]]>其中，k為離散時間變量；特征在傳感器坐標系中一般以極坐標的形式表示：Xfi(k)=ρiθi(k)]]>其中，表示第i個特征的狀態(tài)，即位置；n為特征的個數(shù)；ρi為特征i在極坐標系下的極徑；θi為特征i在極坐標系下的極角；1.4無人偵察機運動模型通過建立一個四自由度的常速運動學(xué)模型來描述無人偵察機的運動變換趨勢，如公式(2)所示：XA(k)＝f(XA(k?1),n(k?1))???(2)無人偵察機XA＝[x?y?z?ψ?u?v?w?r]T，[x,y,z,ψ]為全局坐標系中無人偵察機的位置和艏向，[u,v,w,r]表示運動坐標系A(chǔ)中相應(yīng)的XA、YA、ZA方向的線速度和轉(zhuǎn)艏角速度，n＝[nu,nv,nw,nr]表示以加速度的形式作用在速度上的高斯噪聲；根據(jù)特征狀態(tài)和無人偵察機狀態(tài)XA可以獲得SLAM系統(tǒng)的狀態(tài)X，包括無人偵察機的狀態(tài)XA和特征的狀態(tài)Xf：X＝[XA?Xf]T???(3)xyzψuvwr(k)=x+(uT+nuT22)cosψ-(vT+nvT22)sinψy+(uT+nuT22)sinψ+(vT+nvT22)cosψz+wT+nwT22ψ+rT+nrT22u+nuTv+nvTw+nwTr+nrT(k-1)]]>其中，T為采樣時間；1.5傳感器測量模型(1)速度測量模型速度測量傳感器可提供X、Y、Z三個方向的速度，測量模型為：ZV＝HVX+sV???(4)其中，ZV為速度的測量值；測量矩陣HV為：HV＝[03×4?I3×3?03×1?03×2n]其中，O為全零矩陣，I為單位矩陣；速度測量噪聲sV的協(xié)方差矩陣為：RV=σVu2000σVv2000σVw2]]>其中，為速度u的方差，為速度v的方差，為速度w的方差；(2)高度測量模型通過高度測量傳感器可以獲得無人偵察機的高度，測量模型為：ZH＝HHX+sH???(5)其中，ZH為高度的測量值；測量矩陣HH為：HH＝[0?0?1?0?0?0?0?0?01×2n]高度測量噪聲的sH協(xié)方差矩陣表示為：RH=σH2]]>(3)艏向測量模型通過艏向測量傳感器可以獲得無人偵察機的艏向，測量模型為：ZC＝HCX+sC???(6)其中，ZC為艏向的測量值；測量矩陣HC為：HC＝[0?0?0?1?0?0?0?0?01×2n]艏向測量噪聲sC的協(xié)方差為：RC=σC2]]>(4)環(huán)境測量模型通過環(huán)境感知傳感器可以獲得環(huán)境中特征相對于無人偵查機的距離和方位，測量模型為：ZiA=ρiAθiAT=h(X(k),si)---(7)]]>其中，si為特征測量噪聲；h(X(k),si)=(ρicosθi-x)2+(ρisinθi-y)2arctan(ρisinθi-yρicosθi-x)-ψ+si]]>測量矩陣為：其中，和是點特征在運動坐標系下的參數(shù)表示，ρi和θi是點特征在全局坐標系下的參數(shù)表示；步驟S2：基于步驟S1所建立的無人偵察機SLAM系統(tǒng)模型，執(zhí)行自適應(yīng)EKF的無人偵察機同步定位與構(gòu)圖操作；其中，建立無人偵察機SLAM系統(tǒng)的狀態(tài)方程和觀測方程如下所示，被估計狀態(tài)以及觀測變量與狀態(tài)向量的關(guān)系呈現(xiàn)非線性：X(k)＝f(X(k?1),m(k))?????????????????????????????????(8)Z(k)＝h(X(k),n(k))式中，X(k)是無人偵察機SLAM系統(tǒng)的狀態(tài)向量；Z(k)是系統(tǒng)的觀測向量；m(...
【技術(shù)特征摘要】
【專利技術(shù)屬性】
技術(shù)研發(fā)人員：王晶，王樂鴻，所玉君，崔建飛，李艷玲，
申請(專利權(quán))人：天津津航計算技術(shù)研究所，
類型：發(fā)明
國別省市：天津;12
還沒有人留言評論。發(fā)表了對其他瀏覽者有用的留言會獲得科技券。