雙imu單目視覺組合測(cè)量非慣性系下目標(biāo)物位姿方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及慣性測(cè)量技術(shù)、視覺測(cè)量技術(shù)、組合測(cè)量技術(shù)，尤其涉及雙MU和單目 視覺組合測(cè)量非慣性系下目標(biāo)物位姿方法。
【背景技術(shù)】
[0002]慣性相對(duì)位姿測(cè)量方法將慣性傳感器如加速度計(jì)和陀螺儀直接固連在被測(cè)物體 和運(yùn)動(dòng)參考系上，也可以將多種慣性傳感器組合成慣性測(cè)量單元（MU)后再進(jìn)行安裝。傳感 器輸出敏感到的加速度和角速度，根據(jù)多個(gè)傳感器輸出得到相對(duì)角速度和相對(duì)加速度。再 利用牛頓定律得到相對(duì)位置和角度信息。故原理簡(jiǎn)單、響應(yīng)快速，在傳感器精度保證下瞬間 位姿測(cè)量精度高。不過，相對(duì)角速度依然因陀螺儀漂移存在偏差。而由不同慣性單元的輸出 求相對(duì)加速度時(shí)還需考慮因相對(duì)運(yùn)動(dòng)引起的向心方向加速度、切向方向加速度及科式加速 度，當(dāng)然這些加速度由公式可求。隨著微機(jī)電慣性器件(MEMS加速度計(jì)、陀螺儀）的流行，慣 性測(cè)量所需成本降低、安裝更加小型便捷，所以在工業(yè)生產(chǎn)和科學(xué)研究中得到越來越多的 應(yīng)用。
[0003]視覺位姿測(cè)量方法通常是從拍攝的圖像中提取特征點(diǎn)信息，之后計(jì)算得到目標(biāo)物 相對(duì)于參考坐標(biāo)系的位姿信息。常見的方法有比例正交投影迭代變換算法(POSIT算法）、N 點(diǎn)透視投影算法(PNP算法)等。在視覺測(cè)量中，參考坐標(biāo)系可以是慣性坐標(biāo)系，也可以是非 慣性系，兩種情況下測(cè)量算法大致相同。但是視覺測(cè)量容易受遮擋物干擾、環(huán)境光線干擾， 目標(biāo)物和參考系同時(shí)運(yùn)動(dòng)時(shí)靶標(biāo)更易移出攝像機(jī)視場(chǎng)，同時(shí)其基于圖像處理的算法也讓單 一的視覺測(cè)量無法滿足對(duì)目標(biāo)物的快速實(shí)時(shí)定位。
[0004] 組合位姿測(cè)量技術(shù)是使用多種測(cè)量方式同時(shí)測(cè)量被測(cè)物體的位姿信息，可以結(jié)合 各單一方法的優(yōu)點(diǎn)、彌補(bǔ)各單一方法的不足，常見的組合形式有全球定位系統(tǒng)(GPS)器件與 慣性器件組合、慣性器件與磁強(qiáng)計(jì)組合、慣性器件與電荷耦合元件(CXD)組合等。組合測(cè)量 的核心在于對(duì)各種方式的測(cè)量結(jié)果進(jìn)行評(píng)估，和對(duì)不同來源的測(cè)量數(shù)據(jù)進(jìn)行融合?；パa(bǔ)濾 波器、卡爾曼濾波器等都是有效的數(shù)據(jù)融合方式，其中卡爾曼濾波器在導(dǎo)航、定位等應(yīng)用中 都得到了廣泛使用。而經(jīng)典的卡爾曼濾波器(KF)經(jīng)過修改，又可以演化為拓展卡爾曼濾波 (EKF)、H~濾波、聯(lián)邦卡爾曼濾波等以滿足不同的測(cè)量需求。
[0005] 為充分利用慣性位姿測(cè)量和視覺位姿測(cè)量的特點(diǎn)，實(shí)現(xiàn)揚(yáng)長(zhǎng)補(bǔ)短，本方案設(shè)計(jì)了 一套慣性和視覺組合相對(duì)位姿測(cè)量系統(tǒng)，系統(tǒng)使用了兩個(gè)MU和一組單目視覺。提出了相對(duì) 位姿測(cè)量的實(shí)現(xiàn)方法，可快速、精確得測(cè)量目標(biāo)物相對(duì)運(yùn)動(dòng)參考系的位姿，該方法簡(jiǎn)化后也 可應(yīng)用在物體相對(duì)于慣性系的位姿測(cè)量中。

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

[0006] 為克服現(xiàn)有技術(shù)的不足，準(zhǔn)確測(cè)量非慣性系下目標(biāo)物相對(duì)于運(yùn)動(dòng)參考系的位姿信 息，本發(fā)明采用的技術(shù)方案是，雙MU單目視覺組合測(cè)量非慣性系下目標(biāo)物位姿方法，根據(jù) 兩個(gè)慣性測(cè)量單元IMU輸出的差異得到非慣性系下的目標(biāo)物與運(yùn)動(dòng)參考系之間的相對(duì)角速 度和相對(duì)加速度，相對(duì)角速度經(jīng)過補(bǔ)償并求出相對(duì)姿態(tài)角，相對(duì)加速度經(jīng)過積分求出相對(duì) 位置;利用單目視覺直接測(cè)出在非慣性系中目標(biāo)物上的多特征點(diǎn)靶標(biāo)相對(duì)攝像機(jī)坐標(biāo)系的 旋轉(zhuǎn)矩陣和位置矢量，由旋轉(zhuǎn)矩陣計(jì)算相對(duì)姿態(tài)角;統(tǒng)一兩者測(cè)量結(jié)果到相同的坐標(biāo)系，再 根據(jù)數(shù)據(jù)融合方法最終實(shí)現(xiàn)相對(duì)位姿測(cè)量。
[0007] 目標(biāo)物與運(yùn)動(dòng)參考系之間相對(duì)角速度的測(cè)量值存在漂移誤差，其粗補(bǔ)償步驟是， 采用一個(gè)IMU和特征點(diǎn)靶標(biāo)固連在目標(biāo)物上，其上的IMU坐標(biāo)系作為目標(biāo)系，另一個(gè)IMU和 (XD攝像機(jī)固連在運(yùn)動(dòng)平臺(tái)上，其上的頂U(kuò)坐標(biāo)系作為參考系；
[0008] 將目標(biāo)物靜止時(shí)兩個(gè)頂U(kuò)輸出的角速度做差，再根據(jù)方向余弦矩陣迭代公式、方向 余弦矩陣與歐拉角的關(guān)系得到三個(gè)相對(duì)姿態(tài)角即方位角Φ、俯仰角θ和橫滾角Φ隨時(shí)間的變 化情況;再用三條一元多次函數(shù)曲線擬合，每條曲線對(duì)應(yīng)一個(gè)相對(duì)姿態(tài)角：
[0010]式中，dij為第i條η次曲線的第j次項(xiàng)系數(shù)，τ為MJ采樣周期，1為整數(shù)表示第1個(gè)MJ 采樣周期結(jié)束，3堯36^為第1個(gè)周期結(jié)束時(shí)三個(gè)相對(duì)姿態(tài)角的漂移的估計(jì)，上標(biāo)T 表示轉(zhuǎn)置；目標(biāo)物靜止時(shí)對(duì)相對(duì)姿態(tài)角進(jìn)行m次測(cè)量，采用最小二乘法擬合m次測(cè)量結(jié)果，得 到系數(shù)矩陣：
[0012]
為銬與鳥的內(nèi)積，g為每次測(cè)量時(shí)MU采樣 的周期數(shù)，fP為第i條曲線對(duì)應(yīng)的相對(duì)姿態(tài)角的第P次測(cè)量結(jié)果
私的內(nèi)積;將擬合得到的曲線作為相對(duì)姿態(tài)角漂移的粗估計(jì)，再由各時(shí)刻的粗估計(jì)值逆推 出相對(duì)角速度各時(shí)刻的粗補(bǔ)償值。
[0013] 單一測(cè)量方式位姿信息的輸出過程：由兩個(gè)IMU得到相對(duì)加速度和經(jīng)過補(bǔ)償?shù)南?對(duì)角速度，相對(duì)加速度經(jīng)兩次積分得到相對(duì)位移;相對(duì)角速度經(jīng)離散歐拉角迭代方法得到 相對(duì)姿態(tài)角；單目攝像機(jī)拍攝靶標(biāo)上4個(gè)非共面點(diǎn)，用POSIT算法得到C系到W系間的位姿信 息，將視覺測(cè)量結(jié)果統(tǒng)一為N系到B系的位姿信息，其中，僅用一個(gè)IMU和特征點(diǎn)靶標(biāo)固連在 目標(biāo)物上，其上的頂U(kuò)坐標(biāo)系作為目標(biāo)系B系，另一個(gè)頂U(kuò)和C⑶攝像機(jī)固連在運(yùn)動(dòng)平臺(tái)上，其 上的IMU坐標(biāo)系作為參考系N系，轉(zhuǎn)臺(tái)系R系與運(yùn)動(dòng)平臺(tái)系S系的軸與各自的旋轉(zhuǎn)軸對(duì)齊;世 界坐標(biāo)系W系設(shè)定在靶標(biāo)上;攝像機(jī)坐標(biāo)系C系設(shè)定在攝像機(jī)CCD面上。
[0014] 數(shù)據(jù)融合之前的兩次判定步驟是，判定條件1:P0SIT算法是否報(bào)錯(cuò)，該判定條件旨 在避免采集的圖像受遮擋而缺失靶標(biāo)點(diǎn);判定條件2:
[0015] tr(P)=tr[(#S-ffIMU)(#S-ffIMU) T]?> ει ⑶
[0016] 式中，W#為單目視覺測(cè)量得到的Ν系到Β系的位姿量，WIMU為雙IMU慣性測(cè)量得到的 N系到B系的位姿量， ει為閾值，方陣P的特征值反映了在其特征向量方向上視覺測(cè)量結(jié)果和 雙頂U(kuò)測(cè)量結(jié)果之間的差異的大小，故用Ρ的跡作為判定量，旨在剔除受環(huán)境干擾而質(zhì)量太 差的圖像。
[0017] 數(shù)據(jù)融合具體步驟是，根據(jù)第k次圖像采集時(shí)刻IMU測(cè)量的位姿量和單目視覺 測(cè)量的位姿量11中對(duì)應(yīng)位姿元素的差異大小，在該元素之前乘上k時(shí)刻的關(guān)注因子Lk，由 此構(gòu)造出一個(gè)新的位姿元素的線性組合Z k:
[0019]
表示選取的 狀態(tài)量，其各元素為第k次圖像采集時(shí)刻各相對(duì)位姿量的值，且Xk與和灰的各元素形 式相同，luSLk對(duì)角線上的元素，1 < i <6;根據(jù)本方案搭建的系統(tǒng)，構(gòu)造隨機(jī)非線性離散時(shí) 間系統(tǒng)，系統(tǒng)方程為：
[0021]
狀態(tài)量，T為視覺采樣周期與慣性采樣周期之比，k表示第k次視覺采樣，t+i表示視覺采樣周 期內(nèi)第t + 1次慣性采樣，所以狀態(tài)量表示第
時(shí)刻的相對(duì)位姿量，
為輸入控制量，前三個(gè)元素表示
亥IJB系相對(duì)N系的角速度在B系三軸方向上的量值，Γ為6X6的單位矩陣：
未知的系統(tǒng)噪聲;觀測(cè)方程中，Yk+1S觀測(cè)量，元素形式與Xk+1相同，Yk+1取值于k+Ι時(shí)刻單目 視覺的測(cè)量結(jié)果％?Μ，Η為6X6的單位矩陣，而Vk+1為k+1時(shí)刻未知的觀測(cè)噪聲；用次優(yōu)濾 波估計(jì)出基于上述系統(tǒng)方程和公式(4)的位姿信息，步驟為:先線性化狀態(tài)方程，再設(shè)定變 化的閾值Yk，再由次優(yōu)Η-濾波公式不斷迭代出滿足位姿線性組合最優(yōu)化時(shí)的相對(duì)位姿； 濾波器迭代公式中k+Ι時(shí)刻的估計(jì)誤差方差方陣Pk+i的跡隨k時(shí)刻閾值γ k取值的增大而逐 漸減小并趨于穩(wěn)定，據(jù)此采用while循環(huán)結(jié)構(gòu)并根據(jù)Pk+1斜率變化情況求出P k+1接近穩(wěn)定值 時(shí)的γ k{t:
[0029] 式中，ε2為閾值，取一初值后，八為斤與大于1的放大倍數(shù)α的乘積，/f+1、/丨+1為 根據(jù)％、7【對(duì)應(yīng)求出的估計(jì)誤差方差方陣。
[0030] 雙IMU模塊和單目視覺模塊均測(cè)量有效時(shí)刻，得到融合時(shí)刻非慣性系N系到目標(biāo)系 B系的相對(duì)姿態(tài)角和相對(duì)位置矢量;而在其他時(shí)刻，由雙IMU慣性測(cè)量方式測(cè)量N系到B系的 相對(duì)位姿信息；最后用融合得到的姿態(tài)角分段更新姿態(tài)角漂移的估計(jì)曲線，以實(shí)現(xiàn)修正相 對(duì)角速度的粗補(bǔ)償值;修正的步驟，k時(shí)刻修正量為Δ# = ，xk,,為k時(shí)刻系統(tǒng)狀態(tài) 量的第i個(gè)元素，i取值為1、2、3表示數(shù)據(jù)融合之后的三個(gè)相對(duì)姿態(tài)角，通過漂移曲線降次化 處理，用三個(gè)修正后的漂移量估計(jì)出一條二次曲線，并作為下一段周期內(nèi)的漂移曲線，最終 得到分段更新的姿態(tài)角漂移評(píng)定曲線。
[0031] 本發(fā)明的特點(diǎn)及有益效果是：
[0032] 本發(fā)明設(shè)計(jì)了基于雙MU和單目視覺組合測(cè)量非慣性系下目標(biāo)物位姿的方法，該 方法結(jié)合了慣性位姿測(cè)量短時(shí)測(cè)量準(zhǔn)確高、響應(yīng)快和視覺測(cè)量準(zhǔn)確度穩(wěn)定性好的優(yōu)，彌補(bǔ) 了慣性測(cè)量時(shí)漂問題，解決了視覺位姿測(cè)量方法易受環(huán)境干擾而丟失目標(biāo)或產(chǎn)生較大測(cè)量 誤差的問題，也改善其測(cè)量速度較低的特點(diǎn)。
[0033] 本方案還設(shè)計(jì)了基于上述測(cè)量方法的系統(tǒng)。系統(tǒng)結(jié)構(gòu)簡(jiǎn)單，傳感器安裝簡(jiǎn)易。配合 上位機(jī)進(jìn)行算法處理，可以簡(jiǎn)單、快速、準(zhǔn)確地實(shí)現(xiàn)目標(biāo)物相對(duì)于非慣性的定位。
[