一種基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法
【專(zhuān)利摘要】本發(fā)明公開(kāi)了一種基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法，該方法包括以下步驟：將所述捷聯(lián)慣導(dǎo)系統(tǒng)的推算值XI和所述全球定位系統(tǒng)的觀測(cè)數(shù)據(jù)為XN二者的差值作為所述間接卡爾曼濾波器的觀測(cè)輸入，間接卡爾曼濾波器內(nèi)部構(gòu)造預(yù)測(cè)方程，濾波器輸出為誤差的估計(jì)值此輸出中的部分參數(shù)反饋用于校正所述捷聯(lián)慣導(dǎo)系統(tǒng)以及全球定位系統(tǒng)觀測(cè)計(jì)算中的參數(shù)，以修正二者的誤差計(jì)算，XI與中坐標(biāo)與姿態(tài)等參數(shù)的和作為最終系統(tǒng)各項(xiàng)數(shù)值的估計(jì)值。同傳統(tǒng)的采用直接卡爾曼濾波算法的方法相比,本發(fā)明不但能夠給出更為準(zhǔn)確的位姿估計(jì)結(jié)果,且有效提高機(jī)器人的工作效率。
【專(zhuān)利說(shuō)明】
一種基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于移動(dòng)平臺(tái)位姿估計(jì)方法技術(shù)領(lǐng)域，尤其是涉及一種基于間接卡爾曼濾 波的移動(dòng)平臺(tái)位姿估計(jì)方法。
【背景技術(shù)】
[0002] 對(duì)于移動(dòng)平臺(tái)（例如移動(dòng)機(jī)器人)而言，其核心問(wèn)題就是自動(dòng)計(jì)算移動(dòng)平臺(tái)的位置 和姿態(tài)，其中位置為三維空間的笛卡爾坐標(biāo)(x，y，z)，姿態(tài)為三維空間的姿態(tài)角（Φ，Θ，γ)， 獲取了位姿以后，就可以通過(guò)控制算法對(duì)移動(dòng)平臺(tái)的運(yùn)動(dòng)軌跡進(jìn)行控制，從而使之按照設(shè) 定軌跡進(jìn)行運(yùn)動(dòng)。
[0003] 移動(dòng)平臺(tái)常用的硬件框架是慣導(dǎo)器件（陀螺儀、加速度計(jì)等慣導(dǎo)器件)+觀測(cè)器件 (如GPS、激光、視覺(jué)等觀測(cè)設(shè)備），位姿算法一般采用直接卡爾曼濾波算法，以坐標(biāo)和姿態(tài)角 的直接量作為狀態(tài)變量，觀測(cè)器件的直接量為觀測(cè)值，建立卡爾曼濾波預(yù)測(cè)和觀測(cè)方程進(jìn) 行計(jì)算，其濾波方程比較復(fù)雜，方程中往往包含有不少非線性公式，無(wú)法使用線性卡爾曼濾 波算法進(jìn)行計(jì)算，一般采用擴(kuò)展卡爾曼濾波算法，從而影響到計(jì)算的簡(jiǎn)化和精度，而且對(duì)多 種傳感器的數(shù)據(jù)融合也有不利的影響，因此需要一種更好的方法來(lái)進(jìn)行位姿的估算。
【發(fā)明內(nèi)容】
[0004] 鑒于以上所述現(xiàn)有技術(shù)的缺點(diǎn)，本發(fā)明的目的在于提供一種基于間接卡爾曼濾波 的移動(dòng)平臺(tái)位姿估計(jì)方法，。
[0005] 為了達(dá)到上述發(fā)明目的，解決其技術(shù)問(wèn)題所采用的技術(shù)方案如下：
[0006] 本發(fā)明公開(kāi)了一種基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法，該方法包括以 下步驟：
[0007] 步驟1:構(gòu)建基于間接卡爾曼濾波的移動(dòng)平臺(tái)，該移動(dòng)平臺(tái)包括捷聯(lián)慣導(dǎo)系統(tǒng)、全 球定位系統(tǒng)和間接卡爾曼濾波器；
[0008] 步驟2:根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的陀螺儀的角速度測(cè)量值[ω χ ω y ω ζ]，通過(guò)遞推計(jì)算 旋轉(zhuǎn)矩陣，并根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的里程計(jì)的測(cè)量值計(jì)算速度測(cè)量值在導(dǎo)航系統(tǒng)中的投 影 V.miV.、>'威:、VmD.;
[0009] 步驟3:采用間接卡爾曼濾波器構(gòu)建系統(tǒng)的狀態(tài)預(yù)測(cè)方程和觀測(cè)方程，并利用狀態(tài) 預(yù)測(cè)方程計(jì)算狀態(tài)的預(yù)測(cè)值；
[0010] 步驟4:根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的陀螺儀和里程計(jì)中計(jì)算得到的數(shù)據(jù)推算Xm、ym、zm， 得到捷聯(lián)慣導(dǎo)系統(tǒng)的推算值X:;
[0011] 步驟5:將全球定位系統(tǒng)中的觀測(cè)數(shù)據(jù)XN和步驟4中的捷聯(lián)慣導(dǎo)系統(tǒng)中的推算值X: 做差運(yùn)算，其差值作為間接卡爾曼濾波器輸入的觀測(cè)值，并計(jì)算增益矩陣得到誤差的估計(jì) 值威 1:;
[0012] 步驟6:將誤差估計(jì)值作為反饋，用于修正捷聯(lián)慣導(dǎo)系統(tǒng)的遞推值和全球定位 系統(tǒng)的觀測(cè)數(shù)據(jù)，并將捷聯(lián)慣導(dǎo)系統(tǒng)的推算值X:和誤差估計(jì)值結(jié)合得到導(dǎo)航系統(tǒng)所需 的移動(dòng)平臺(tái)的位置和姿態(tài)；
[0013]步驟7:返回步驟1，循環(huán)計(jì)算，直到達(dá)到導(dǎo)航系統(tǒng)設(shè)定的停止條件為止。
[0014]進(jìn)一步的，步驟2具體包括以下步驟：
[0015] 根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的陀螺儀的角速度測(cè)量值[ωχωγωζ]，通過(guò)四元數(shù)遞推計(jì)算 得出旋轉(zhuǎn)矩陣G ,遞推計(jì)算公式如下：
[0016] 巧(幻=成1)，初始旋轉(zhuǎn)矩陣/
[0019] 進(jìn)一步通過(guò)旋轉(zhuǎn)矩陣C；：計(jì)算出慣導(dǎo)遞推的 ，計(jì)算公式如下：
[0020]
[0021] 公式2中，^、<、<分別代表在導(dǎo)航坐標(biāo)系中的角度預(yù)測(cè)值、y、z表示繞x、y、z 軸旋轉(zhuǎn)的角度，即分別繞北東地旋轉(zhuǎn)的角度;Rn代表旋轉(zhuǎn)矩陣q中的第1行第1列的數(shù)據(jù)， r12代表旋轉(zhuǎn)矩陣q中的第1行第2列的數(shù)據(jù)，r13代表旋轉(zhuǎn)矩陣?：中的第1行第3列的數(shù)據(jù)， R23代表旋轉(zhuǎn)矩陣0；中的第2行第3列的數(shù)據(jù)，R33代表旋轉(zhuǎn)矩陣q中的第3行第3列的數(shù)據(jù)；
[0022] 并根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的里程計(jì)測(cè)量值計(jì)算速度測(cè)量值在導(dǎo)航系中的投影 V?，.V、VmE、Vmt>，計(jì)算公式如下：
[0023]
[0024] 公式3中，V:、<E、分別為里程計(jì)測(cè)量值計(jì)算速度測(cè)量值在導(dǎo)航系北、東和地 三個(gè)方向上的速度投影。
[0025] 進(jìn)一步的，步驟3具體包括以下步驟：
[0026]首先，定義系統(tǒng)狀態(tài)變量：
[0027] 5vn、3ve、3vd、^*.、知2、心、37、52和31(八
[0028] 其中，δνΝ、δνΕ、δν?分別為移動(dòng)平臺(tái)在北、東和地三個(gè)方向的速度誤差， y分別為移動(dòng)平臺(tái)在X、y和Ζ三個(gè)方向的姿態(tài)誤差，δχ、δγ、δζ分別為移動(dòng)平臺(tái) 在x、y和z三個(gè)方向的坐標(biāo)誤差，以及δΚΑ為里程計(jì)的刻度誤差；
[0029] 其次，構(gòu)建依據(jù)上述系統(tǒng)狀態(tài)變量的狀態(tài)預(yù)測(cè)方程：
[0030]
[0031]公式4中，VN、VE、VD分別為移動(dòng)平臺(tái)在北、東和地三個(gè)方向上的速度，且考慮到車(chē)輪 運(yùn)動(dòng)存在打滑的情況，qo~q3即為車(chē)輪打滑時(shí)速度誤差的方差；
[0035] 公式5-7中，考慮到陀螺儀的零偏不穩(wěn)定性因素，q3~q5即為角度誤差方差；
[0036] 5x(k+l) =5vN(k)*T (公式 8)
[0037] 5y(k+l) =5vE(k)*T (公式 9)
[0038] 5z(k+l) =5vD(k)*T (公式 10)
[0039] 公式8-10中，T為兩次觀測(cè)之間的間隔時(shí)間；
[0040] KA(k+l) = 5KA(k)+q6 (公式11)
[0041] 公式11中，考慮誤差系數(shù)存在漂移的情況，q6即為里程計(jì)刻度誤差系數(shù)的方差；
[0042] 再者，構(gòu)建vn、ve、vd的速度誤差與x、y、z方向上的坐標(biāo)誤差的觀測(cè)方程如下：
[0043] Ζι(1〇 = δνΝ(1〇+ξ〇 (公式 12)
[0044] Z2(k) = δνΕ(1〇+ξι (公式 13)
[0045] Z3(k) = 5vD(k)+|2 (公式 14)
[0046] Z4(k) = δχ(1〇+ξ3 (公式 15)
[0047] Z5(k) = 5y(k)+|4 (公式 16)
[0048] Z6(k) = δζ(1〇+ξ5 (公式 17)
[0049] 公式12-17中，考慮到GPS觀測(cè)值存在一定誤差，其中ξ〇~ξ5即為觀測(cè)方差。
[0050] 進(jìn)一步的，上述公式4的具體推算過(guò)程如下：
[0051]根據(jù)運(yùn)載系統(tǒng)的比力方程：
[0052]
[0053]將比力方程向?qū)Ш阶鴺?biāo)系中投影得到：
[0054]
[0056] 比較公式19與公式20,并省略二階以上小量，得到：
[0057]
[0058] 將比力中的向心加速度項(xiàng)考慮作為誤差，可認(rèn)為·Γ=#，δΚΑ為里程計(jì)誤差系數(shù)并 忽略地球自轉(zhuǎn)角速度，因此得到：
[0059]
[0060] 進(jìn)一步的，步驟4具體包括以下步驟：
[0061] 根據(jù)已經(jīng)推算得到的V:、進(jìn)一步計(jì)算義^^^計(jì)算方法如下：
[0062]
[0063]公式22中，xm、ym、zm分別代表在導(dǎo)航坐標(biāo)系中的xyz坐標(biāo)，即北東地方向的坐標(biāo)； <#、<?、分別為里程計(jì)測(cè)量值計(jì)算速度測(cè)量值在導(dǎo)航系北、東和地三個(gè)方向上的速 度投影。
[0064] 進(jìn)一步的，步驟5具體包括以下步驟：
[0065]用全球定位系統(tǒng)所測(cè)得的3個(gè)速度觀測(cè)數(shù)據(jù)VN、VE、VD和3個(gè)位置觀測(cè)數(shù)據(jù)x、y、z與 捷聯(lián)慣導(dǎo)系統(tǒng)中的推算值乂:做差運(yùn)算，其差值作為間接卡爾曼濾波器輸入的觀測(cè)值，并計(jì) 算增益矩陣得到誤差的估計(jì)值：
[0067] 其中，δνΝ、δνΕ、δν?分別為移動(dòng)平臺(tái)在北、東和地三個(gè)方向的速度誤差， ，0'與分別為移動(dòng)平臺(tái)在X、y和ζ三個(gè)方向的姿態(tài)誤差，δ X、δ y、δ ζ分別為移動(dòng)平臺(tái) 在x、y和ζ三個(gè)方向的坐標(biāo)誤差，以及δΚΑ為里程計(jì)的刻度誤差。
[0068] 進(jìn)一步的，步驟6具體包括以下步驟：
[0069]將誤差估計(jì)值作為反饋，用于修正捷聯(lián)慣導(dǎo)系統(tǒng)的遞推值和全球定位系統(tǒng)的 觀測(cè)數(shù)據(jù)，并將捷聯(lián)慣導(dǎo)系統(tǒng)的推算值X:和誤差估計(jì)值結(jié)合得到導(dǎo)航系統(tǒng)所需的移動(dòng) 平臺(tái)的位置和姿態(tài)，即：
[0076] 公式23-28中，€、<、€分別代表在導(dǎo)航坐標(biāo)系中的角度預(yù)測(cè)值、y、z表示繞 x、y、z軸旋轉(zhuǎn)的角度，即分別繞北東地旋轉(zhuǎn)的角度;xm、ym、zm分別代表在導(dǎo)航坐標(biāo)系中的xyz 坐標(biāo)，即北東地方向的坐標(biāo)。
[0077] 進(jìn)一步的，步驟7中所述導(dǎo)航系統(tǒng)設(shè)定的停止條件為誤差值等于0。
[0078] 本發(fā)明由于采用以上技術(shù)方案，使之與現(xiàn)有技術(shù)相比，具有以下的優(yōu)點(diǎn)和積極效 果：
[0079] 1.在本發(fā)明中，以慣導(dǎo)器件陀螺儀、里程計(jì)為狀態(tài)遞推依據(jù)，全球定位系統(tǒng)GPS的 觀測(cè)數(shù)據(jù)減去捷聯(lián)慣導(dǎo)系統(tǒng)的遞推值作為觀測(cè)值，共同組成間接卡爾曼濾波的狀態(tài)和輸出 方程。計(jì)算公式得到簡(jiǎn)化且更為直觀，卡爾曼濾波計(jì)算更快速，計(jì)算結(jié)果精度也得到有效提 尚；
[0080] 2.同傳統(tǒng)的采用直接卡爾曼濾波算法的方法相比，本發(fā)明不但能夠給出更為準(zhǔn)確 的估計(jì)結(jié)果，且拓寬了系統(tǒng)應(yīng)用范圍；
[0081] 3.本發(fā)明位姿估計(jì)方法定位準(zhǔn)確且易于實(shí)現(xiàn)，在移動(dòng)機(jī)器人的仿真過(guò)程中，移動(dòng) 機(jī)器人的位姿估計(jì)更加精確，有效提高機(jī)器人的工作效率。
【附圖說(shuō)明】
[0082] 為了更清楚地說(shuō)明本發(fā)明實(shí)施例的技術(shù)方案，下面將對(duì)實(shí)施例描述中所需要使用 的附圖作簡(jiǎn)單的介紹。顯而易見(jiàn)，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對(duì)于本領(lǐng) 域技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以根據(jù)這些附圖獲得其他的附圖。附 圖中：
[0083] 圖1為本發(fā)明一種基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法中的系統(tǒng)總體框 架示意圖。
【具體實(shí)施方式】
[0084] 以下將結(jié)合本發(fā)明的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整的描述 和討論，顯然，這里所描述的僅僅是本發(fā)明的一部分實(shí)例，并不是全部的實(shí)例，基于本發(fā)明 中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒(méi)有做出創(chuàng)造性勞動(dòng)的前提下所獲得的所有其他實(shí)施 例，都屬于本發(fā)明的保護(hù)范圍。
[0085] 如圖1所示為系統(tǒng)總體框架示意圖，所述捷聯(lián)慣導(dǎo)系統(tǒng)的推算值為X:，所述全球定 位系統(tǒng)的觀測(cè)數(shù)據(jù)為Xn，二者的差值作為所述間接卡爾曼濾波器的觀測(cè)輸入，間接卡爾曼 濾波器內(nèi)部構(gòu)造預(yù)測(cè)方程，濾波器輸出為誤差的估計(jì)值此輸出中的部分參數(shù)反饋用 于校正所述捷聯(lián)慣導(dǎo)系統(tǒng)以及全球定位系統(tǒng)觀測(cè)計(jì)算中的參數(shù)，以修正二者的誤差計(jì)算， &與八戈1中坐標(biāo)與姿態(tài)等參數(shù)的和作為最終系統(tǒng)各項(xiàng)數(shù)值的估計(jì)值。本實(shí)施例中，觀測(cè)器件 使用的是全球定位系統(tǒng)，但采用激光、視覺(jué)等觀測(cè)設(shè)備也能達(dá)到同樣的技術(shù)效果，也是本發(fā) 明所保護(hù)的技術(shù)范圍。
[0086] 圖中，間接卡爾曼濾波器不直接估計(jì)導(dǎo)航系統(tǒng)的狀態(tài)量(如坐標(biāo)、姿態(tài)等），而是估 計(jì)捷聯(lián)慣導(dǎo)系統(tǒng)與真值之間的誤差，因此以全球定位系統(tǒng)的觀測(cè)數(shù)據(jù)與捷聯(lián)慣導(dǎo)系統(tǒng)的推 算值的誤差作為觀測(cè)值求解卡爾曼濾波結(jié)果。
[0087]本實(shí)施例具體的步驟如下：
[0088] 本發(fā)明公開(kāi)了一種基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法，該方法包括以 下步驟：
[0089] 步驟1:構(gòu)建基于間接卡爾曼濾波的移動(dòng)平臺(tái)，該移動(dòng)平臺(tái)包括捷聯(lián)慣導(dǎo)系統(tǒng)、全 球定位系統(tǒng)和間接卡爾曼濾波器；
[0090] 步驟2:根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的陀螺儀的角速度測(cè)量值[ω χ ω y ω z]，通過(guò)遞推計(jì)算 旋轉(zhuǎn)矩陣并根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的里程計(jì)的測(cè)量值計(jì)算速度測(cè)量值在導(dǎo)航系統(tǒng)中的投 Μ/ η ； η . τη ^ VmEs YmD ?
[0091] 步驟3:采用間接卡爾曼濾波器構(gòu)建系統(tǒng)的狀態(tài)預(yù)測(cè)方程和觀測(cè)方程，并利用狀態(tài) 預(yù)測(cè)方程計(jì)算狀態(tài)的預(yù)測(cè)值；
[0092] 步驟4:根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的陀螺儀和里程計(jì)中計(jì)算得到的數(shù)據(jù)推算Xm、ym、zm， 得到捷聯(lián)慣導(dǎo)系統(tǒng)的推算值心；
[0093] 步驟5:將全球定位系統(tǒng)中的觀測(cè)數(shù)據(jù)XN和步驟4中的捷聯(lián)慣導(dǎo)系統(tǒng)中的推算值X: 做差運(yùn)算，其差值作為間接卡爾曼濾波器輸入的觀測(cè)值，并計(jì)算增益矩陣得到誤差的估計(jì) 值
[0094] 步驟6:將誤差估計(jì)值Δ±Ι作為反饋，用于修正捷聯(lián)慣導(dǎo)系統(tǒng)的遞推值和全球定位 系統(tǒng)的觀測(cè)數(shù)據(jù)，并將捷聯(lián)慣導(dǎo)系統(tǒng)的推算值X:和誤差估計(jì)值Δ?：##合得到導(dǎo)航系統(tǒng)所需 的移動(dòng)平臺(tái)的位置和姿態(tài)；
[0095]步驟7:返回步驟1，循環(huán)計(jì)算，直到達(dá)到導(dǎo)航系統(tǒng)設(shè)定的停止條件為止。
[0096] 具體實(shí)施例中，步驟2具體包括以下步驟：
[0097] 根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的陀螺儀的角速度測(cè)量值[ωχωγωζ]，通過(guò)四元數(shù)遞推計(jì)算 得出旋轉(zhuǎn)矩陣G，遞推計(jì)算公式如下：
[0098] 二肩：左-i):c?a -1)，初始旋轉(zhuǎn)矩陣αο) ; /
[0101] 進(jìn)一步通過(guò)旋轉(zhuǎn)矩陣q：計(jì)算出慣導(dǎo)遞推的<、if、計(jì)算公式如下：
[0102]
[0103] 公式2中，<、<、<分別代表在導(dǎo)航坐標(biāo)系中的角度預(yù)測(cè)值、y、z表示繞x、y、z 軸旋轉(zhuǎn)的角度，即分別繞北東地旋轉(zhuǎn)的角度;Rn代表旋轉(zhuǎn)矩陣中的第1行第1列的數(shù)據(jù)， r12代表旋轉(zhuǎn)矩陣q*中的第1行第2列的數(shù)據(jù)，r13代表旋轉(zhuǎn)矩陣q：中的第1行第3列的數(shù)據(jù)，R 23 代表旋轉(zhuǎn)矩陣q：中的第2行第3列的數(shù)據(jù)，R33代表旋轉(zhuǎn)矩陣中的第3行第3列的數(shù)據(jù)；
[0104] 并根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的里程計(jì)測(cè)量值計(jì)算速度測(cè)量值在導(dǎo)航系中的投影 vLv、<*、計(jì)算公式如下：
[0105]
[0106] 公式3中，<,v、<E、分別為里程計(jì)測(cè)量值計(jì)算速度測(cè)量值在導(dǎo)航系北、東和地 三個(gè)方向上的速度投影。
[0107] 具體實(shí)施例中，步驟3具體包括以下步驟：
[0108] 首先，定義系統(tǒng)狀態(tài)變量：
[0109] δνΝ、δνΕ、δν?)、?>ν?·、0妙、0碑、3叉、5丫、52和31(八
[0110] 其中，δνΝ、δνΕ、δν?分別為移動(dòng)平臺(tái)在北、東和地三個(gè)方向的速度誤差， d'<m、#勢(shì)s 分別為移動(dòng)平臺(tái)在x、y和ζ三個(gè)方向的姿態(tài)誤差，δχ、δγ、δζ分別為移動(dòng)平臺(tái) 在x、y和ζ三個(gè)方向的坐標(biāo)誤差，以及δΚΑ為里程計(jì)的刻度誤差；
[0111] 其次，構(gòu)建依據(jù)上述系統(tǒng)狀態(tài)變量的狀態(tài)預(yù)測(cè)方程：
[0112]
[0113]公式4中，VN、VE、VD分別為移動(dòng)平臺(tái)在北、東和地三個(gè)方向上的速度，且考慮到車(chē)輪 運(yùn)動(dòng)存在打滑的情況，qo~q3即為車(chē)輪打滑時(shí)速度誤差的方差；
[0117]公式5-7中，考慮到陀螺儀的零偏不穩(wěn)定性因素，q3~q5即為角度誤差方差；
[0118] 5x(k+l) =5vN(k)*T (公式 8)
[0119] 5y(k+l) =5vE(k)*T (公式 9)
[0120] 5z(k+l) =5vD(k)*T (公式 10)
[0121 ] 公式8-10中，T為兩次觀測(cè)之間的間隔時(shí)間；
[0122] KA(k+l) = 5KA(k)+q6 (公式 11)
[0123] 公式11中，考慮誤差系數(shù)存在漂移的情況，q6即為里程計(jì)刻度誤差系數(shù)的方差；
[0124] 再者，構(gòu)建VN、VE、VD的速度誤差與x、y、z方向上的坐標(biāo)誤差的觀測(cè)方程如下：
[0125] zi(k) = δνΝ(1〇+ξο (公式 12)
[0126] Z2(k) = δνΕ(1〇+ξι (公式 13)
[0127] Z3(k) = 5vD(k)+|2 (公式 14)
[0128] Z4(k) = δχ(1〇+ξ3 (公式 15)
[0129] Z5(k) = 5y(k)+|4 (公式 16)
[0130] Z6(k) = δζ(1〇+ξ5 (公式 17)
[0131 ]公式12-17中，考慮到GPS觀測(cè)值存在一定誤差，其中ξο~ξ5即為觀測(cè)方差。
[0132] 具體實(shí)施例中，上述公式4的具體推算過(guò)程如下：
[0133] 根據(jù)運(yùn)載系統(tǒng)的比力方程：
[0134]
[0135] 將比力方程向?qū)Ш阶鴺?biāo)系中投影得到：
[0136]
[0137]
[0138] 比較公式19與公式20,并省略二階以上小量，得到：
[0139]
[0140] 將比力中的向心加速度項(xiàng)考慮作為誤差，可認(rèn)為.Γ=ι/?，δΚΑ為里程計(jì)誤差系數(shù)并 忽略地球自轉(zhuǎn)角速度，因此得到：
[0141]
[0142] 具體實(shí)施例中，步驟4具體包括以下步驟：
[0143] 根據(jù)已經(jīng)推算得到的ν;;,Λ.、V=.E、V;;,D進(jìn)一步計(jì)算乂 111、7111、2111，計(jì)算方法如下：
[0144]
[0145] 公式22中，Xm、ym、Zm分別代表在導(dǎo)航坐標(biāo)系中的xyz坐標(biāo)，即北東地方向的坐標(biāo)； Vlv、<,E、V:,D分別為里程計(jì)測(cè)量值計(jì)算速度測(cè)量值在導(dǎo)航系北、東和地三個(gè)方向上的速 度投影。
[0146] 具體實(shí)施例中，步驟5具體包括以下步驟：
[0147] 用全球定位系統(tǒng)所測(cè)得的3個(gè)速度觀測(cè)數(shù)據(jù)VN、VE、VD和3個(gè)位置觀測(cè)數(shù)據(jù)x、y、z與 捷聯(lián)慣導(dǎo)系統(tǒng)中的推算值乂:做差運(yùn)算，其差值作為間接卡爾曼濾波器輸入的觀測(cè)值，并計(jì) 算增益矩陣得到誤差的估計(jì)值：
[0148] 5vn、5ve、5vd、<5^?'、.谷約'、<^碑、5叉、55^、52和51(八
[0149] 其中，δνΝ、δνΕ、δν?分別為移動(dòng)平臺(tái)在北、東和地三個(gè)方向的速度誤差， _、勒y、匆Ζ分別為移動(dòng)平臺(tái)在 χ、3^Ρζ三個(gè)方向的姿態(tài)誤差，δχ、δγ、δζ分別為移動(dòng)平臺(tái) 在x、y和ζ三個(gè)方向的坐標(biāo)誤差，以及δΚ Α為里程計(jì)的刻度誤差。
[0150] 具體實(shí)施例中，步驟6具體包括以下步驟：
[0151] 將誤差估計(jì)值Δ&作為反饋，用于修正捷聯(lián)慣導(dǎo)系統(tǒng)的遞推值和全球定位系統(tǒng)的 觀測(cè)數(shù)據(jù)，并將捷聯(lián)慣導(dǎo)系統(tǒng)的推算值X:和誤差估計(jì)值結(jié)合得到導(dǎo)航系統(tǒng)所需的移動(dòng) 平臺(tái)的位置和姿態(tài)，即：
[0152]
[0158] 公式23-28中，<、<、<分別代表在導(dǎo)航坐標(biāo)系中的角度預(yù)測(cè)值、y、z表示繞 x、y、Z軸旋轉(zhuǎn)的角度，即分別繞北東地旋轉(zhuǎn)的角度;Xm、ym、Zm分別代表在導(dǎo)航坐標(biāo)系中的xyz 坐標(biāo)，即北東地方向的坐標(biāo)。
[0159] 進(jìn)一步的，步驟7中所述導(dǎo)航系統(tǒng)設(shè)定的停止條件為誤差值等于0。
[0160] 本發(fā)明中，以慣導(dǎo)器件陀螺儀、里程計(jì)為狀態(tài)遞推依據(jù)，全球定位系統(tǒng)GPS的觀測(cè) 數(shù)據(jù)減去捷聯(lián)慣導(dǎo)系統(tǒng)的遞推值作為觀測(cè)值，共同組成間接卡爾曼濾波的狀態(tài)和輸出方 程。計(jì)算公式得到簡(jiǎn)化且更為直觀，卡爾曼濾波計(jì)算更快速，計(jì)算結(jié)果精度也得到有效提 高。同傳統(tǒng)的采用直接卡爾曼濾波算法的方法相比，本發(fā)明不但能夠給出更為準(zhǔn)確的估計(jì) 結(jié)果，且拓寬了系統(tǒng)應(yīng)用范圍。使得移動(dòng)機(jī)器人的定位更為準(zhǔn)確且易于實(shí)現(xiàn)，在移動(dòng)機(jī)器人 的仿真過(guò)程中，移動(dòng)機(jī)器人的位姿估計(jì)更加精確，能有效提高機(jī)器人的工作效率。
[0161]以上所述，僅為本發(fā)明較佳的【具體實(shí)施方式】，但本發(fā)明的保護(hù)范圍并不局限于此， 任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，可輕易想到的變化或替換， 都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。因此，本發(fā)明的保護(hù)范圍應(yīng)該以權(quán)利要求的保護(hù)范圍 為準(zhǔn)。
【主權(quán)項(xiàng)】
1. 一種基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法，其特征在于，該方法包括以下 步驟： 步驟1:構(gòu)建基于間接卡爾曼濾波的移動(dòng)平臺(tái)，該移動(dòng)平臺(tái)包括捷聯(lián)慣導(dǎo)系統(tǒng)、全球定 位系統(tǒng)和間接卡爾曼濾波器； 步驟2:根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的陀螺儀的角速度測(cè)量值[ωχ coy ωζ]，通過(guò)遞推計(jì)算旋 轉(zhuǎn)矩陣C，并根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的里程計(jì)的測(cè)量值計(jì)算速度測(cè)量值在導(dǎo)航系統(tǒng)中的投影 η η η 步驟3:采用間接卡爾曼濾波器構(gòu)建系統(tǒng)的狀態(tài)預(yù)測(cè)方程和觀測(cè)方程，并利用狀態(tài)預(yù)測(cè) 方程計(jì)算狀態(tài)的預(yù)測(cè)值； 步驟4:根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的陀螺儀和里程計(jì)中計(jì)算得到的數(shù)據(jù)推算Xm、ym、zm，得到 捷聯(lián)慣導(dǎo)系統(tǒng)的推算值XI; 步驟5:將全球定位系統(tǒng)中的觀測(cè)數(shù)據(jù)ΧΝ和步驟4中的捷聯(lián)慣導(dǎo)系統(tǒng)中的推算值X:做差 運(yùn)算，其差值作為間接卡爾曼濾波器輸入的觀測(cè)值，并計(jì)算增益矩陣得到誤差的估計(jì)值 步驟6:將誤差估計(jì)值々^^作為反饋，用于修正捷聯(lián)慣導(dǎo)系統(tǒng)的遞推值和全球定位系統(tǒng) 的觀測(cè)數(shù)據(jù)，并將捷聯(lián)慣導(dǎo)系統(tǒng)的推算值X:和誤差估計(jì)值結(jié)合得到導(dǎo)航系統(tǒng)所需的移 動(dòng)平臺(tái)的位置和姿態(tài)； 步驟7:返回步驟1，循環(huán)計(jì)算，直到達(dá)到導(dǎo)航系統(tǒng)設(shè)定的停止條件為止。2. 如權(quán)利要求1所述的基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法，其特征在于，步 驟2具體包括以下步驟： 根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的陀螺儀的角速度測(cè)量值[ωχ ωζ]，通過(guò)四元數(shù)遞推計(jì)算得 出旋轉(zhuǎn)矩陣q，遞推計(jì)算公式如下： Μ =我k - \ -1)，初始旋轉(zhuǎn)矩陣= j進(jìn)一步通過(guò)旋轉(zhuǎn)矩陣G計(jì)算出慣導(dǎo)遞推的計(jì)算公式如下：(公式2) 公式2中，少Γ分別代表在導(dǎo)航坐標(biāo)系中的角度預(yù)測(cè)值、y、z表示繞x、y、z軸旋 轉(zhuǎn)的角度，即分別繞北東地旋轉(zhuǎn)的角度;Rn代表旋轉(zhuǎn)矩陣Q中的第1行第1列的數(shù)據(jù)，R12代 表旋轉(zhuǎn)矩陣q中的第1行第2列的數(shù)據(jù)，R 13代表旋轉(zhuǎn)矩陣中的第1行第3列的數(shù)據(jù)，R23代表 旋轉(zhuǎn)矩陣G中的第2行第3列的數(shù)據(jù)，R 33代表旋轉(zhuǎn)矩陣中的第3行第3列的數(shù)據(jù)； 并根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)中的里程計(jì)測(cè)量值計(jì)算速度測(cè)量值在導(dǎo)航系中的投影 V%v、VmE、計(jì)算公式如下： 9(公式3) 公式3中，vLv、<,E、分別為里程計(jì)測(cè)量值計(jì)算速度測(cè)量值在導(dǎo)航系北、東和地三個(gè) 方向上的速度投影。3.如權(quán)利要求1所述的基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法，其特征在于，步 驟3具體包括以下步驟： 首先，定義系統(tǒng)狀態(tài)變量： 其中，δνΝ、δνΕ、δν[)分別為移動(dòng)平臺(tái)在北、東和地三個(gè)方向的速度誤差，?^Λ-、8ψ? 分別為移動(dòng)平臺(tái)在x、y和ζ三個(gè)方向的姿態(tài)誤差，δχ、δγ、δζ分別為移動(dòng)平臺(tái)在x、y和ζ三個(gè)方 向的坐標(biāo)誤差，以及為里程計(jì)的刻度誤差； 其次，構(gòu)建依據(jù)上述系統(tǒng)狀態(tài)變量的狀態(tài)預(yù)測(cè)方程：(公式4) 公式4中，VN、VE、VD分別為移動(dòng)平臺(tái)在北、東和地三個(gè)方向上的速度，且考慮到車(chē)輪運(yùn)動(dòng) 存在打滑的情況，qo~q3即為車(chē)輪打滑時(shí)速度誤差的方差； Ui/? 1- I) - σψ-'wt-Lh(7) 公式5-7中，考慮到陀螺儀的零偏不穩(wěn)定性因素，q3~q5即為角度誤差方差； 5x(k+l) = 5vN(k)*T (公式 8) 5y(k+l) = 5vE(k)*T (公式 9) 5z(k+l) = 5vD(k)*T (公式 10) 公式8-10中，T為兩次觀測(cè)之間的間隔時(shí)間； KA(k+l) =5KA(k)+q6 (公式 11) 公式11中，考慮誤差系數(shù)存在漂移的情況，q6即為里程計(jì)刻度誤差系數(shù)的方差； 再者，構(gòu)建VN、VE、VD的速度誤差與x、y、z方向上的坐標(biāo)誤差的觀測(cè)方程如下： zi(k) =δνΝ(1〇+ξ〇 (公式 12) Z2(k) =δνΕ(1〇+ξι (公式 13) Z3(k) =5vD(k)+|2 (公式 14) Z4(k) =δχ(1〇+ξ3 (公式 15) Z5(k) =5y(k)H4 (公式 16) Z6(k) =δζ(1〇+ξ5 (公式 17) 公式12-17中，考慮到GPS觀測(cè)值存在一定誤差，其中ξ〇~|5即為觀測(cè)方差。4. 如權(quán)利要求3所述的基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法，其特征在于，上 述公式4的具體推算過(guò)程如下： 根據(jù)運(yùn)載系統(tǒng)的比力方程：(公式18) 將比力方程向?qū)Ш阶鴺?biāo)系中投影得到：比較公式19與公式20,并省略二階以上小量，得到：將比力中的向心加速度項(xiàng)考慮作為誤差，可認(rèn)為尸=1>b，ska為里程計(jì)誤差系數(shù)并忽略 地球自轉(zhuǎn)角速度，因此得到：5. 如權(quán)利要求1所述的基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法，其特征在于，步 驟4具體包括以下步驟： 根據(jù)已經(jīng)推算得到的_V=E、V_!1d進(jìn)一步計(jì)算1111、71 11、2111，計(jì)算方法如下：公式22中，xm、ym、zm分別代表在導(dǎo)航坐標(biāo)系中的xy Z坐標(biāo)，即北東地方向的坐標(biāo)； C、<E、V:分別為里程計(jì)測(cè)量值計(jì)算速度測(cè)量值在導(dǎo)航系北、東和地三個(gè)方向上的速 度投影。6. 如權(quán)利要求1所述的基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法，其特征在于，步 驟5具體包括以下步驟： 用全球定位系統(tǒng)所測(cè)得的3個(gè)速度觀測(cè)數(shù)據(jù)VN、VE、VD和3個(gè)位置觀測(cè)數(shù)據(jù)x、y、z與捷聯(lián) 慣導(dǎo)系統(tǒng)中的推算值乂:做差運(yùn)算，其差值作為間接卡爾曼濾波器輸入的觀測(cè)值，并計(jì)算增 益矩陣得到誤差的估計(jì)值： 其中，δνΝ、δνΕ、δνΒ分別為移動(dòng)平臺(tái)在北、東和地三個(gè)方向的速度誤差，％:、 分別為移動(dòng)平臺(tái)在x、y和z三個(gè)方向的姿態(tài)誤差，δχ、δγ、δζ分別為移動(dòng)平臺(tái)在x、y和z三個(gè)方 向的坐標(biāo)誤差，以及為里程計(jì)的刻度誤差。7. 如權(quán)利要求1所述的基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法，其特征在于，步 驟6具體包括以下步驟： 將誤差估計(jì)值Δ^Ι作為反饋，用于修正捷聯(lián)慣導(dǎo)系統(tǒng)的遞推值和全球定位系統(tǒng)的觀測(cè) 數(shù)據(jù)，并將捷聯(lián)慣導(dǎo)系統(tǒng)的推算值心和誤差估計(jì)值結(jié)合得到導(dǎo)航系統(tǒng)所需的移動(dòng)平臺(tái) 的位置和姿態(tài)，即：公式23-28中，識(shí)Γ分別代表在導(dǎo)航坐標(biāo)系中的角度預(yù)測(cè)值、y、z表示繞x、y、z 軸旋轉(zhuǎn)的角度，即分別繞北東地旋轉(zhuǎn)的角度;Xm、ym、Zm分別代表在導(dǎo)航坐標(biāo)系中的xyz坐標(biāo)， 即北東地方向的坐標(biāo)。8. 如權(quán)利要求1所述的基于間接卡爾曼濾波的移動(dòng)平臺(tái)位姿估計(jì)方法，其特征在于，步 驟7中所述導(dǎo)航系統(tǒng)設(shè)定的停止條件為誤差值等于0。
【文檔編號(hào)】G01C21/20GK105865452SQ201610284802
【公開(kāi)日】2016年8月17日
【申請(qǐng)日】2016年4月29日
【發(fā)明人】龐文堯, 陶熠昆, 黃鴻, 章海兵, 王培建
【申請(qǐng)人】浙江國(guó)自機(jī)器人技術(shù)有限公司