[發(fā)明專利]一種便攜式慣性導(dǎo)航定位桿及其定位定姿方法

有效

• 申請?zhí)枺?/td>: 201911099403.5
• 申請日: 2019-11-12
• 公開（公告）號: CN110763229B
• 公開（公告）日: 2022-04-15
• 發(fā)明（設(shè)計）人: 牛小驥;陳起金;周禹昆;曠儉;李雨;郭若南;賴昌鑫
• 申請（專利權(quán)）人: 武漢大學(xué)
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20;G01C25/00
• 代理公司: 湖北武漢永嘉專利代理有限公司 42102
• 代理人: 劉琰

1.一種便攜式慣性導(dǎo)航定位桿，其特征在于，包括載體桿、測量傳感器和數(shù)據(jù)處理單元；其中：

測量傳感器固定安裝在載體桿上，測量傳感器包括：慣性測量單元、零速探測傳感器；慣性測量單元，用于測量三維比力和三維角速度信息；零速探測傳感器，固定安裝在載體桿上，通過判斷零速探測傳感器輸出是否超出設(shè)定閾值進(jìn)行零速探測；

數(shù)據(jù)處理單元用于處理測量傳感器實時傳輸?shù)臏y量信息，結(jié)合零速探測信息，通過慣導(dǎo)機械編排算法和卡爾曼濾波進(jìn)行數(shù)據(jù)融合和定位定姿解算；

載體桿的形狀包括：桿狀載體、球體、圓柱體或長方體；

零速探測傳感器包括：壓力傳感器、慣性傳感器、彈性開關(guān)、用戶手動開關(guān)；

慣性測量單元包括三軸加速度計和三軸陀螺儀；

便攜式慣性導(dǎo)航定位桿的定位定姿方法包括以下步驟：

S1、對慣性導(dǎo)航定位桿進(jìn)行定位初始化；

S2、定位過程中，慣性導(dǎo)航定位桿間歇性地獲得零速修正機會；

S3、探測慣性導(dǎo)航定位桿的零速時段，判斷慣性導(dǎo)航定位桿一端與探測點地面是否接觸并保持靜止，若是，則記錄觸地時間段為零速時段；

S4、獲取慣性導(dǎo)航定位桿觸地時間段內(nèi)的零速觀測值，與慣性導(dǎo)航系統(tǒng)進(jìn)行數(shù)據(jù)融合，實現(xiàn)組合定位定姿解算；

步驟S1中進(jìn)行慣性導(dǎo)航定位桿定位初始化的具體方法為：

S11、進(jìn)行定位或定位算法包括慣性導(dǎo)航位置、速度和姿態(tài)的解算；

S12、慣性導(dǎo)航定位桿的定位初始化包括位置初始化、速度初始化、姿態(tài)初始化；

步驟S2中慣性導(dǎo)航定位桿間歇性地獲得零速修正機會的具體方法為：

S21、操作慣性導(dǎo)航定位桿，使慣性導(dǎo)航定位桿的某質(zhì)點相對于地球或地面固定點的速度為零；

S22、不要求慣性測量單元本身或慣性導(dǎo)航定位桿整體速度為零，只要與慣性測量單元固聯(lián)的慣性導(dǎo)航定位桿的某一部位速度為零，則可視為零速修正的機會；

步驟S3中探測慣性導(dǎo)航定位桿零速時段的方法包括：

在慣性導(dǎo)航定位桿與地物接觸點處安裝壓力傳感器：通過探測該點壓力值的變化來感知其與地面固定點是否接觸并保持靜止；當(dāng)壓力傳感器的壓力值超過某一預(yù)先設(shè)定的閾值時判定慣性導(dǎo)航定位桿該部位與地面固定點接觸，記錄相應(yīng)的壓力大于閾值的時間段，作為零速修正時段；

在慣性導(dǎo)航定位桿內(nèi)設(shè)置慣性測量單元：通過慣性測量單元原始數(shù)據(jù)的變化來判斷慣性導(dǎo)航定位桿與地面接觸點是否處于零速狀態(tài)；

在慣性導(dǎo)航定位桿與地物接觸點處安裝彈性開關(guān)：當(dāng)慣性導(dǎo)航定位桿觸地時彈性開關(guān)所受彈力增大，慣性導(dǎo)航定位桿離地時開關(guān)所受彈力減小，通過判斷開關(guān)的彈力變化來判斷慣性導(dǎo)航定位桿是否觸地，處于零速狀態(tài)；

在慣性導(dǎo)航定位桿與地物接觸點處安裝手動開關(guān)：當(dāng)慣性導(dǎo)航定位桿觸地時用戶打開手動開關(guān)，慣性導(dǎo)航定位桿離地時刻關(guān)閉手動開關(guān)，記錄定位桿的觸地時間段；

步驟S4中進(jìn)行數(shù)據(jù)融合實現(xiàn)組合定位定姿解算的具體方法為：

S41、當(dāng)慣性導(dǎo)航定位桿處于零速時段時，記錄零速時段內(nèi)的零速觀測值，將零速觀測值與慣性導(dǎo)航系統(tǒng)通過濾波的形式實現(xiàn)數(shù)據(jù)融合，進(jìn)行組合定位解算；

S42、構(gòu)建零速修正的觀測方程；慣性測量單元速度與慣性導(dǎo)航定位桿觸地點之間的速度關(guān)系表示為：

其中，b表示慣性測量單元載體坐標(biāo)系，即b系；n表示當(dāng)?shù)厮阶鴺?biāo)系，也稱導(dǎo)航坐標(biāo)系，n系；l^b為慣性測量單元中心指向定位桿與地面接觸點的三維向量在b系下的投影；為b系相對于n系的旋轉(zhuǎn)角速度在b系下的投影；為n系與b系之間的坐標(biāo)變換矩陣；為慣性測量單元的速度在n系下的投影；為定位桿觸地點的速度；

根據(jù)慣性導(dǎo)航系統(tǒng)的導(dǎo)航結(jié)果推算零速點的速度為：

零速點的實際觀測為：

其中，n表示零速點的速度，其值不是絕對為零；

慣性導(dǎo)航定位桿零速修正的觀測方程寫作：

S43、當(dāng)慣性導(dǎo)航定位桿處于零速狀態(tài)時，慣性導(dǎo)航定位桿觸地點處的位置保持不變，將位置保持不變的觸地點的位置信息作為慣性導(dǎo)航的外部觀測，抑制慣性導(dǎo)航系統(tǒng)的位置發(fā)散；

S44、組合定位定姿是將零速信息作為外部輔助與慣性導(dǎo)航解算進(jìn)行數(shù)據(jù)融合；數(shù)據(jù)融合方法包括卡爾曼濾波、擴展卡爾曼濾波、無跡卡爾曼濾波法、粒子濾波法、序貫最小二乘法、非線性優(yōu)化算法或人工神經(jīng)網(wǎng)絡(luò)法；

步驟S43的將觸地點的位置信息作為慣性導(dǎo)航的外部觀測的具體方法為：

S431、將零速開始時刻的位置作為外部位置觀測與慣性導(dǎo)航系統(tǒng)進(jìn)行數(shù)據(jù)融合；

S432、將慣性導(dǎo)航定位桿看作繞觸地點轉(zhuǎn)動的剛體，結(jié)合慣性導(dǎo)航系統(tǒng)的姿態(tài)和桿臂l^b推算出慣性測量單元的速度和位置，替換慣性導(dǎo)航的速度和位置更新。