[發(fā)明專利]一種基于信號(hào)站的位置和姿態(tài)估計(jì)方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201611040794.X
• 申請(qǐng)日: 2016-11-11
• 公開(kāi)（公告）號(hào): CN106525050B
• 公開(kāi)（公告）日: 2019-04-09
• 發(fā)明（設(shè)計(jì)）人: 馬旭;趙雪嬌;王志強(qiáng);李艷秋
• 申請(qǐng)（專利權(quán)）人: 北京理工大學(xué)
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20;G01S5/02;G01S5/10
• 代理公司: 北京理工大學(xué)專利中心 11120
• 代理人: 郭德忠;李愛(ài)英

本發(fā)明提供一種基于信號(hào)站的位置和姿態(tài)估計(jì)方法。本方法采用待測(cè)物體上的均勻直線陣和壓縮感知技術(shù)，實(shí)時(shí)接收信號(hào)站信號(hào)并計(jì)算所有信號(hào)對(duì)于待測(cè)物體的達(dá)到角度。根據(jù)各個(gè)信號(hào)站的位置和所有信號(hào)的到達(dá)角度，采用擴(kuò)展卡爾曼濾波技術(shù)對(duì)物體的位置和姿態(tài)參數(shù)進(jìn)行實(shí)時(shí)估計(jì)。本發(fā)明的方法可以通過(guò)逐一求解不同信號(hào)的到達(dá)角，去除不同信號(hào)之間的耦合及相互干擾，降低不同信號(hào)之間的相互影響，獲得較高的到達(dá)角估計(jì)精度；此外本方法采用壓縮感知技術(shù)能夠有效降低接收信號(hào)時(shí)的采樣率和信號(hào)接收器的成本，降低位置和姿態(tài)估計(jì)算法的計(jì)算量，提高運(yùn)算效率；同時(shí)，本發(fā)明采用擴(kuò)展卡爾曼濾波技術(shù)，可進(jìn)一步有效提高位置和姿態(tài)參數(shù)的估計(jì)精度。

技術(shù)領(lǐng)域

本發(fā)明屬于定位和導(dǎo)航技術(shù)領(lǐng)域，尤其涉及一種基于信號(hào)站的位置和姿態(tài)估計(jì)方法。

背景技術(shù)

位置和姿態(tài)估計(jì)技術(shù)在航空航天、移動(dòng)式遙控裝置、無(wú)人機(jī)導(dǎo)航或無(wú)人駕駛、室內(nèi)導(dǎo)航等諸多領(lǐng)域均有應(yīng)用。位置估計(jì)的主要目標(biāo)是確定物體的空間坐標(biāo)，而姿態(tài)估計(jì)的主要目標(biāo)是確定連體坐標(biāo)系與參考坐標(biāo)系之間的相對(duì)轉(zhuǎn)角。特別是在某些無(wú)人機(jī)和室內(nèi)導(dǎo)航等應(yīng)用場(chǎng)景中，需要定位導(dǎo)航系統(tǒng)具有成本低、功耗低、穩(wěn)定性高、體積小等特點(diǎn)。另外，對(duì)于精確的實(shí)時(shí)導(dǎo)航系統(tǒng)，還需要位置和姿態(tài)估計(jì)方法具有解算精度高、計(jì)算復(fù)雜度低的特點(diǎn)。目前，滿足上述要求的導(dǎo)航系統(tǒng)主要采用全球定位系統(tǒng)(global positioning system，簡(jiǎn)稱GPS)、慣性導(dǎo)航系統(tǒng)(inertial navigation system，簡(jiǎn)稱INS)、磁力計(jì)、地面信號(hào)站，以及視覺(jué)或光學(xué)系統(tǒng)進(jìn)行位置和姿態(tài)估計(jì)。但是，GPS系統(tǒng)要求具備較高的時(shí)鐘同步精度和信號(hào)采樣頻率，有時(shí)還會(huì)因?yàn)槎鄰椒瓷洹o(wú)線電干擾等原因而無(wú)法使用；低成本INS系統(tǒng)的測(cè)量數(shù)據(jù)會(huì)因?yàn)檎`差積累而發(fā)生漂移；采用磁力計(jì)估計(jì)姿態(tài)必須借助于地球磁場(chǎng)分布數(shù)據(jù)，而且單純使用磁力計(jì)無(wú)法獲取物體的位置信息；基于信號(hào)站的位置和姿態(tài)估計(jì)精度也會(huì)受到信號(hào)能量衰減、無(wú)線電干擾等因素的影響；而視覺(jué)或光學(xué)系統(tǒng)通常需要配合參考地圖，才能計(jì)算出物體的位置和姿態(tài)數(shù)據(jù)，并且容易受到障礙物遮擋等環(huán)境因素的影響。

相關(guān)文獻(xiàn)(Technical Report ARL-CR-650,US Army Research Laboratory,Jun.2010)提出了一種基于地面信號(hào)站的位置和姿態(tài)估計(jì)方法，通過(guò)地面信號(hào)到待測(cè)物體的到達(dá)角，以及上一測(cè)量時(shí)刻的物體姿態(tài)信息，依次計(jì)算出物體在地固坐標(biāo)系內(nèi)的坐標(biāo)和距離地面的高度，之后再根據(jù)物體的位置坐標(biāo)，借助磁力計(jì)，計(jì)算出物體的姿態(tài)角。如此循環(huán)交替更新物體的位置和姿態(tài)參數(shù)，從而實(shí)現(xiàn)位置和姿態(tài)的實(shí)時(shí)估計(jì)。但是以上方法具有以下不足：第一、上述方法沒(méi)有在位置和姿態(tài)參數(shù)的解算過(guò)程中采用有效的濾波手段，因此估計(jì)誤差較大；第二、上述方法借助磁力計(jì)的測(cè)量數(shù)據(jù)估計(jì)姿態(tài)參數(shù)，因此需要地球磁場(chǎng)分布的先驗(yàn)數(shù)據(jù)；第三、該方法采用高采樣率的原始地面信號(hào)計(jì)算其相對(duì)于待測(cè)物體的到達(dá)角，參與運(yùn)算的數(shù)據(jù)量較大、計(jì)算復(fù)雜度較高，影響位置和姿態(tài)估計(jì)的實(shí)時(shí)性。針對(duì)上述位置和姿態(tài)估計(jì)的技術(shù)需求，以及現(xiàn)有方法的局限性，有必要將多種位置和姿態(tài)估計(jì)方法與濾波技術(shù)和信號(hào)處理技術(shù)相結(jié)合，提高現(xiàn)有方法的參數(shù)估計(jì)精度和運(yùn)算效率。

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

本發(fā)明的目的是提供一種基于信號(hào)站的位置和姿態(tài)估計(jì)方法。該方法采用壓縮感知(compressive sensing，簡(jiǎn)稱CS)技術(shù)降低接收信號(hào)站信號(hào)(下文簡(jiǎn)稱“信號(hào)”)時(shí)的采樣率，壓縮信號(hào)維度，降低接收器成本，提高位置和姿態(tài)估計(jì)方法的運(yùn)算效率。另外，該方法還采用擴(kuò)展卡爾曼濾波(extended Kalman filter，簡(jiǎn)稱EKF)技術(shù)提高了位置和姿態(tài)的估計(jì)精度。

實(shí)現(xiàn)本發(fā)明的技術(shù)方案如下：

一種基于信號(hào)站的位置和姿態(tài)估計(jì)方法，具體步驟為：

步驟1、確定地固坐標(biāo)系的三個(gè)相互正交的坐標(biāo)軸分別為X軸、Y軸和Z軸，連體坐標(biāo)系的三個(gè)相互正交的坐標(biāo)軸分別為X_b軸、Y_b軸和Z_b軸；