一種基于自適應ekf的無人偵察機同步定位與構圖方法
【技術領域】
[0001] 本發明屬于無人機技術領域，具體涉及一種基于自適應EKF的無人偵察機同步定 位與構圖方法。
【背景技術】
[0002] 現代戰爭中，戰場環境復雜，及時掌握戰場情況對取得戰爭勝利起著決定性的作 用。無人偵察機應運而生，其能夠潛入敵區上空進行偵查，為作戰指揮中心提供實時的敵區 圖像和信息，使得指揮官及時掌握敵軍情況，進而制定合理的作戰計劃。其中，小型無人偵 查機以其體積小、重量輕、造價低、無傷亡等優勢已經成為目前研究的熱點。無人偵察機在 執行任務過程中要求其具有續航時間長、導航精度高、不易被敵方發現等特性。其中高精度 的導航是無人偵察機成功完成任何任務的基本前提，因而恰當的導航方法是無人偵察機完 成任務成敗的關鍵所在。
[0003] 導航是指無人偵察機機借助外部信號或通過自身攜帶的傳感器判斷自身位置，從 而指導其航行、完成預定任務的過程。目前無人偵察機的導航方法主要分為兩種，非自主導 航以及自主導航。
[0004] 非自主導航方法主要是衛星導航，利用GPS、歐米茄、羅蘭、北斗等，能夠獲得位置、 速度、時間等導航信息，且精度不隨時間推移而降低。然而GPS無法提供無人偵察機的姿態 信息，此外，目前只有GPS是覆蓋全球的，這就使得無人偵察機在執行大范圍任務時難免受 制于人，并且應用環境的適應性較差，容易受到干擾。自主導航不依賴外界信號源，僅僅通 過自身攜帶的速度、姿態及環境感知傳感器即可實現相對準確的導航，但是對于導航算法 的要求較高，是目前無人偵察機導航領域的研究重點及難點。
[0005] 自主導航方法主要有航位推算、視覺導航、地球物理導航、多普勒導航、慣性導航、 同步定位與構圖等。航位推算具有較強的自主性、保密性及抗干擾性，但是其推算誤差隨著 時間迅速積累，不適合長航時無人偵察機導航；視覺導航具有獨立性、準確性，但是只能獲 得相對的運動狀態信息；地球物理導航具有無源性、無輻射、隱蔽性強，但是需要大量的地 磁數據，實時性受計算機處理能力限制，且受地形，天氣影響；多普勒導航自主性好、測速精 度高、抗干擾性強，但是其工作時必須發射電波，隱蔽性不好；慣性導航，精度高、隱蔽性好， 短期內具有很高的導航定位精度，但慣導系統初始對準較為困難，且造價比較高。
[0006] 同步定位與地圖構建（SimultaneousLocalizationandMapping，SLAM)技術的 提出，一定程度上緩解了上述導航方法中存在的不足。SLAM是在無人偵察機自身位置不確 定，航位推算的過程中位置誤差逐漸累積的情況下，利用自身攜帶的環境感知傳感器反復 探測環境中特征，從而完成自身及特征位置的校正，同時構建環境地圖，無需預知地形信息 或外部輔助定位設備即可以得到較為可靠的無人偵察機位置信息及環境地圖信息，詳細描 述如圖1所示。
【發明內容】
[0007] (一）要解決的技術問題
[0008] 本發明要解決的技術問題是：如何將自適應濾波方法與SLAM方法相結合以解決 無人偵查機自主導航過程中存在的位置誤差累計、環境適應性差、初始對準困難及隱蔽性 差等問題。
[0009] (二）技術方案
[0010] 為解決上述技術問題，本發明提供一種基于自適應EKF的無人偵察機同步定位與 構圖方法，該方法包括如下步驟：
[0011] 步驟S1 :對無人偵察機SLAM系統建模；
[0012] 1.1地圖模型
[0013] 采用特征地圖來構建環境地圖模型，將環境中的目標用幾何原型來描述；
[0014] 1.2坐標系統
[0015] 由于無人偵察機所攜帶的傳感器與環境特征之間的感知行為發生在傳感器坐標 系中，速度的測量發生在無人偵察機運動坐標系中，而最終環境地圖的構建需要表示在全 局地圖中，所以建立全局坐標系，無人偵察機運動坐標系及傳感器坐標系，并表明他們之間 的關系，定義全局坐標系，G為地心，X指向正北，Y指向正東，Z垂直于X平面 并指向G;定義AXaYaZa為無人偵察機運動坐標系，A為無人偵察機質心，XA指向機頭， 直于機頭，ΖΑ垂直于ΧΑΥΑ平面并指向A;定義SXsYsZs為環境感知傳感器坐標系，S為傳感器 質心，Xs指向傳感器電磁波發射方向，Ys垂直于電磁波發射方向，Zs垂直于XSYS平面并指向 S;三個坐標系均符合右手定則；
[0016] 1.3特征模型
[0017] 根據公式（1)，定義特征的運動學模型為：
[0018] Xfi (k) =Xfi (k~l)i= 1···η (1)
[0019] 其中，k為離散時間變量；
[0020] 特征在傳感器坐標系中一般以極坐標的形式表示：
[0021]
[0022] 其中，Xfl表示第i個特征的狀態，即位置；η為特征的個數；
[0023] Pi為特征i在極坐標系下的極徑；Θi為特征i在極坐標系下的極角；
[0024] 1. 4無人偵察機運動模型
[0025] 通過建立一個四自由度的常速運動學模型來描述無人偵察機的運動變換趨勢，如 公式（2)所示：
[0026] XA (k) =f(XA (k-1),n(k~l)) (2)
[0027] 無人偵察機XA=[xyzΦuvwr]T，[x,y,z,Φ]為全局坐標系中無人偵察機 的位置和艏向，[u，v，w，r]表示運動坐標系A中相應的XA、YA、ZA方向的線速度和轉艏角速 度，n= [nu,nv,nw,nj表示以加速度的形式作用在速度上的高斯噪聲；
[0028] 根據特征狀態Xfl和無人偵察機狀態X&可以獲得SLAM系統的狀態X，包括無人偵 察機的狀態XA和特征的狀態Xf:
[0029] X=[XAXf]T ⑶
[0030]
[0031] 其中，T為采樣時間；
[0032] 1. 5傳感器測量模型
[0033] (1)速度測量模型
[0034] 速度測量傳感器可提供X、Υ、Ζ三個方向的速度，測量模型為：
[0035] Zv=HvX+sv (4)
[0036] 其中，Zv為速度的測量值；
[0037] 測量矩陣比為：
[0038] Ηγ -[03X4Ι3Χ303Χ103Χ2η]
[0039] 其中，0為全零矩陣，I為單位矩陣；
[0040] 速度測量噪聲sv的協方差矩陣為：
[0041]
[0042] 其中，cjI為速度u的方差，4，為速度v的方差，為速度w的方差；
[0043] (2)高度測量模型
[0044] 通過高度測量傳感器可以獲得無人偵察機的高度，測量模型為：
[0045] Zh=HhX+sh (5)
[0046] 其中，ZH為高度的測量值；
[0047] 測量矩陣Hh為：
[0048] Hh= [0 0 1 0 0 0 0 0 01X2n]
[0049] 高度測量噪聲的sH協方差矩陣表示為：
[0050]
[0051] ⑶艏向測量模型
[0052] 通過艏向測量傳感器可以獲得無人偵察機的艏向，測量模型為：
[0053] Zc=HcX+sc (6)
[0054] 其中，Zc為艏向的測量值；
[0055] 測量矩陣氏為：
[0056] Hc= [0 00100000 1X2n]
[0057] 艏向測量噪聲sc的協方差為：
[0058] Rc 二
[0059] (4)環境測量模型
[0060] 通過環境感知傳感器可以獲得環境中特征相對于無人偵查機的距離和方位，測量 模型為：
[0061]
：(J);
[0062] 其中，Sl為特征測量噪聲；
[0063]
[0064] 測量矩陣為：
[0065]
[0066] 其中，/^和貧4是點特征在運動坐標系下的參數表示，pJPΘ1是點特征在全局 坐標系下的參數表示；
[0067