一種不規(guī)則外形物體空間姿態(tài)動(dòng)態(tài)測(cè)量方法
【專利摘要】本發(fā)明涉及運(yùn)動(dòng)物體空間姿態(tài)動(dòng)態(tài)測(cè)量【技術(shù)領(lǐng)域】，具體公開了一種不規(guī)則外形物體空間姿態(tài)動(dòng)態(tài)測(cè)量方法。該方法包括：1）在被測(cè)物上設(shè)置光學(xué)靶標(biāo)，并利用激光跟蹤儀對(duì)所有光學(xué)靶標(biāo)進(jìn)行全局校準(zhǔn)；2）利用相機(jī)測(cè)量系統(tǒng)標(biāo)定技術(shù)對(duì)測(cè)量系統(tǒng)進(jìn)行標(biāo)定，并控制左右兩相機(jī)同步采集測(cè)量圖像，并通過(guò)圖像處理技術(shù)提取光學(xué)靶標(biāo)的圖像坐標(biāo)；3）利用步驟1、2所獲得的光學(xué)靶標(biāo)在被測(cè)物坐標(biāo)系下的三維坐標(biāo)和在測(cè)量坐標(biāo)系下的三維坐標(biāo)，獲得旋轉(zhuǎn)矩陣，獲得被測(cè)物的三維空間姿態(tài)角。該測(cè)量方法，可以測(cè)量非軸對(duì)稱的不規(guī)則外形的空間物體的瞬時(shí)空間三維姿態(tài)角；在測(cè)量范圍2m×2m×2m的空間中，測(cè)量頻率1000Hz的測(cè)量條件下，測(cè)量精度可達(dá)到空間角合成均方根誤差小于0.05°。
【專利說(shuō)明】-種不規(guī)則外形物體空間姿態(tài)動(dòng)態(tài)測(cè)量方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于運(yùn)動(dòng)物體空間姿態(tài)動(dòng)態(tài)測(cè)量【技術(shù)領(lǐng)域】，具體涉及一種不規(guī)則外形物體 空間姿態(tài)動(dòng)態(tài)測(cè)量方法。

【背景技術(shù)】
[0002] 空間姿態(tài)動(dòng)態(tài)測(cè)量指利用傳感器獲取空間物體H維運(yùn)動(dòng)姿態(tài)參數(shù)，包括物體自身 坐標(biāo)系相對(duì)于某固定參考坐標(biāo)系的偏航角、俯仰角和滾轉(zhuǎn)角。由于物體處于運(yùn)動(dòng)當(dāng)中，接觸 式測(cè)量方法不能解決上述測(cè)量問(wèn)題，需要采用非接觸式測(cè)量方法。其中，光學(xué)測(cè)量是主流的 空間姿態(tài)動(dòng)態(tài)測(cè)量技術(shù)。現(xiàn)有的測(cè)量物體空間姿態(tài)的技術(shù)主要有利用光電經(jīng)締儀圖像的中 軸線法、利用多相機(jī)的二維平面測(cè)量方法和利用單站圖像基于數(shù)學(xué)迭代優(yōu)化方法等。
[0003] (1)利用光電經(jīng)締儀圖像的中軸線法
[0004] 利用光電經(jīng)締儀圖像的中軸線法是首先假設(shè)被測(cè)物體是軸對(duì)稱的，利用光電經(jīng)締 儀獲取被測(cè)物體清晰輪廓的圖像，利用圖像處理技術(shù)對(duì)圖像進(jìn)行分析處理，提取被測(cè)物的 兩條輪廓邊界的直線方程，進(jìn)而求出被測(cè)物體的中軸線方程，此直線與經(jīng)締儀照相系統(tǒng)的 光也唯一確定了一個(gè)空間平面；利用兩臺(tái)經(jīng)締儀測(cè)量就能得到兩個(gè)平面，兩平面在空間相 交，其交線就是被測(cè)物體的空間中軸線。得到物體的空間中軸線，便可進(jìn)一步求出被測(cè)物體 的偏航角和俯仰角。
[0005] (2)利用多相機(jī)的二維平面測(cè)量方法
[0006] 利用多相機(jī)的二維平面測(cè)量方法是首先在被測(cè)物體上設(shè)置光學(xué)祀標(biāo)，制造成像特 征，利用多臺(tái)相機(jī)在空間不同位置對(duì)被測(cè)物體成像，獲取光學(xué)祀標(biāo)的圖像，利用兩個(gè)光學(xué)祀 標(biāo)的質(zhì)也構(gòu)建一條直線，解算直線與相機(jī)坐標(biāo)軸間的角度，進(jìn)而求解被測(cè)物體的空間姿態(tài) 角在與相機(jī)成像平面平行的平面內(nèi)的角分量，利用多臺(tái)相機(jī)便可W獲得被測(cè)物體在指定方 向上的姿態(tài)角變化。
[0007] (3)利用單站圖像的數(shù)學(xué)迭代優(yōu)化法
[0008] 利用單站圖像的數(shù)學(xué)迭代優(yōu)化法是利用單個(gè)影像經(jīng)締儀或相機(jī)獲取被測(cè)物體的 圖像，通過(guò)圖像處理技術(shù)提取被測(cè)物輪廓上的邊界角點(diǎn)，由攝影測(cè)量理論中共線條件約束 可知，像面上一點(diǎn)a與相機(jī)光也0的連線確定空間一條直線Oa，像點(diǎn)對(duì)應(yīng)的物點(diǎn)A必在直 線Oa上。取被測(cè)目標(biāo)上多于H個(gè)特征點(diǎn)，且已知任意兩點(diǎn)間的距離，利用數(shù)學(xué)迭代優(yōu)化的 方法便可求解出被測(cè)物體的空間H維姿態(tài)。
[0009] 現(xiàn)有的技術(shù)方法在面對(duì)不規(guī)則外形的物體空間H維姿態(tài)動(dòng)態(tài)測(cè)量問(wèn)題時(shí)，均表現(xiàn) 出一定局限性：
[0010] (1)利用光電經(jīng)締儀圖像的中軸線法僅適用于輪廓線是直線的軸對(duì)稱物體，對(duì)被 測(cè)物的形狀規(guī)則性依賴性很強(qiáng)，對(duì)于非軸對(duì)稱的不規(guī)則形體，算法穩(wěn)定性和適應(yīng)性受到很 大影響，測(cè)量精度嚴(yán)重降低，甚至出現(xiàn)方法失效的情況；另外，該方法完全依靠物體自身的 輪廓圖像，由于輪廓提取精度是影響測(cè)量精度的關(guān)鍵因素之一，保證有效的測(cè)量精度對(duì)圖 像處理技術(shù)提出很高要求，相應(yīng)的算法復(fù)雜性顯著提高。
[0011] (2)利用多相機(jī)的二維平面測(cè)量方法本質(zhì)上屬于二維測(cè)量，獲得的測(cè)量結(jié)果是被 測(cè)物空間H維姿態(tài)角在某固定平面上的投影分量，投影平面是與成像相機(jī)像面平行的平 面。因此，該方法的測(cè)量結(jié)果不能真實(shí)的反映被測(cè)物的H維姿態(tài)角，需要在不同位置設(shè)置測(cè) 量相機(jī)獲取感興趣的方向上的姿態(tài)角，或通過(guò)多個(gè)方向的姿態(tài)角合成被測(cè)物體的H維空間 姿態(tài)角數(shù)據(jù)。
[0012] (3)利用單站圖像的數(shù)學(xué)迭代優(yōu)化方法是基于相機(jī)成像模型結(jié)合空間距離約束建 立數(shù)學(xué)方程，利用數(shù)學(xué)方法迭代優(yōu)化求解被測(cè)物空間姿態(tài)參數(shù)。該方法的測(cè)量精度嚴(yán)重依 賴相機(jī)內(nèi)參數(shù)、崎變參數(shù)的標(biāo)定精度和空間約束距離的精度，與圖像處理被測(cè)物邊界角點(diǎn) 的精度密切相關(guān)。目前能夠?qū)崿F(xiàn)的精度不高，受圖像質(zhì)量影響顯著。


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

[0013] 本發(fā)明的目的在于提供一種不規(guī)則外形物體空間姿態(tài)動(dòng)態(tài)測(cè)量方法，能夠適用于 非軸對(duì)稱的不規(guī)則外形的空間物體；通過(guò)設(shè)置測(cè)量特征，能夠容易得到很高的特征提取精 度，能夠?qū)崿F(xiàn)真正的H維姿態(tài)角測(cè)量，且該方法對(duì)圖像處理算法的要求低，實(shí)現(xiàn)簡(jiǎn)單，精度 商。
[0014] 本發(fā)明的技術(shù)方案如下：一種不規(guī)則外形物體空間姿態(tài)動(dòng)態(tài)測(cè)量方法，該方法具 體包括如下步驟：
[0015] 步驟1、在被測(cè)物上設(shè)置光學(xué)祀標(biāo)，并利用激光跟蹤儀對(duì)所有光學(xué)祀標(biāo)進(jìn)行全局校 準(zhǔn)；
[0016] 在被測(cè)物體表面的明顯位置設(shè)有光學(xué)祀標(biāo)，并保證被測(cè)物體運(yùn)動(dòng)過(guò)程中左右兩臺(tái) 相機(jī)均能夠同時(shí)看到至少H個(gè)光學(xué)祀標(biāo)；
[0017] 步驟2、利用相機(jī)測(cè)量系統(tǒng)標(biāo)定技術(shù)對(duì)測(cè)量系統(tǒng)進(jìn)行標(biāo)定，并利用計(jì)算機(jī)控制左右 兩相機(jī)同步采集測(cè)量圖像，并通過(guò)圖像處理技術(shù)提取光學(xué)祀標(biāo)的圖像坐標(biāo)；
[0018] 步驟2. 1、利用相機(jī)測(cè)量系統(tǒng)標(biāo)定技術(shù)標(biāo)定出左右兩臺(tái)測(cè)量相機(jī)的內(nèi)參數(shù)和外參 數(shù)；
[0019] 步驟2. 2、通過(guò)同步控制器控制兩臺(tái)測(cè)量相機(jī)W固定測(cè)量頻率采集測(cè)量圖像，并利 用質(zhì)也法提取光學(xué)祀標(biāo)的圖像坐標(biāo)；
[0020] 步驟2. 3、獲取光學(xué)祀標(biāo)在兩臺(tái)相機(jī)的測(cè)量圖像中對(duì)應(yīng)的質(zhì)也坐標(biāo)后，對(duì)成像鏡頭 的崎變進(jìn)行校正補(bǔ)償；
[0021] 步驟2. 4、完成崎變校正后，獲得光學(xué)祀標(biāo)在測(cè)量坐標(biāo)系下的H維坐標(biāo)；
[0022] 步驟3、利用步驟1、步驟2所獲得的光學(xué)祀標(biāo)在被測(cè)物坐標(biāo)系下的H維坐標(biāo)和在 測(cè)量坐標(biāo)系下的H維坐標(biāo)，獲得被測(cè)物坐標(biāo)系相對(duì)測(cè)量坐標(biāo)系的旋轉(zhuǎn)矩陣，從而獲得被測(cè) 物的H維空間姿態(tài)角。
[0023] 所述的步驟1中利用激光跟蹤儀對(duì)光學(xué)祀標(biāo)進(jìn)行全局校準(zhǔn)的具體步驟為：
[0024] 利用激光跟蹤儀測(cè)量H個(gè)光學(xué)祀標(biāo)的空間H維坐標(biāo)，并將激光跟蹤儀坐標(biāo)系設(shè)為 標(biāo)定坐標(biāo)系〇E-X義Z。，光學(xué)祀標(biāo)在標(biāo)定坐標(biāo)系下的H維坐標(biāo)為（X。（i)，Y。（i)，Z。（〇)， (i=l，2,3);利用激光跟蹤儀測(cè)量被測(cè)物體的坐標(biāo)系基準(zhǔn)，建立被測(cè)物坐標(biāo)系與標(biāo)定坐標(biāo)系 的轉(zhuǎn)換關(guān)系，將光學(xué)祀標(biāo)在標(biāo)定坐標(biāo)系下的H維坐標(biāo)轉(zhuǎn)換為在被測(cè)物坐標(biāo)系下的H維坐標(biāo) (Xt (i)，Yt (i)，Zt (〇)，（i=l，2,3)〇
[0025] 所述的步驟2. 1中標(biāo)定左右兩臺(tái)測(cè)量相機(jī)的內(nèi)參數(shù)和外參數(shù)分別為：
[0026] 利用平面標(biāo)定法標(biāo)定出左右兩臺(tái)測(cè)量相機(jī)的內(nèi)參數(shù)（(；<"，心"，fyW，fyW，k/"，k2" )，Pi("，P2。)）（i=l，2 ;1代表左相機(jī)，2代表右相機(jī)）和外參數(shù)R、T，其中，Cx("，Cy。)為i相機(jī) 的鏡頭主點(diǎn)位置；fyW，fyW為i相機(jī)的有效焦距；k/"，為i相機(jī)的鏡頭徑向崎變系數(shù). Pi("，P2")為i相機(jī)鏡頭切向崎變系數(shù)；R為旋轉(zhuǎn)矩陣；T為平移矩陣。
[0027] 所述的步驟2. 2的具體步驟為：
[0028] 通過(guò)同步控制器控制兩臺(tái)測(cè)量相機(jī)W固定測(cè)量頻率采集測(cè)量圖像，通過(guò)直方圖分 析、闊值分割圖像處理手段濾除圖像背景噪聲，獲得僅含有光學(xué)祀標(biāo)的理想圖像，利用質(zhì)也 法提取光學(xué)祀標(biāo)的圖像坐標(biāo)（Xi。)，yi(")和（Xr。)，y/") (i=l，2, 3)，其中，XI。)和yi。）為左 相機(jī)測(cè)量的第i個(gè)光學(xué)祀標(biāo)的圖像坐標(biāo)，x/"，y/"為右相機(jī)測(cè)量的第i個(gè)光學(xué)祀標(biāo)的圖像 坐標(biāo)。
[0029] 所述的步驟2. 3的具體步驟為：
[0030] 獲取光學(xué)祀標(biāo)在兩臺(tái)相機(jī)的測(cè)量圖像中對(duì)應(yīng)的質(zhì)也坐標(biāo)后，對(duì)成像鏡頭的崎變進(jìn) 行校正補(bǔ)償，校正模型為：

【權(quán)利要求】
1. 一種不規(guī)則外形物體空間姿態(tài)動(dòng)態(tài)測(cè)量方法，其特征在于：該方法具體包括如下步 驟： 步驟1、在被測(cè)物上設(shè)置光學(xué)祀標(biāo)，并利用激光跟蹤儀對(duì)所有光學(xué)祀標(biāo)進(jìn)行全局校準(zhǔn)； 在被測(cè)物體表面的明顯位置設(shè)有光學(xué)祀標(biāo)，并保證被測(cè)物體運(yùn)動(dòng)過(guò)程中左右兩臺(tái)相機(jī) 均能夠同時(shí)看到至少H個(gè)光學(xué)祀標(biāo)； 步驟2、利用相機(jī)測(cè)量系統(tǒng)標(biāo)定技術(shù)對(duì)測(cè)量系統(tǒng)進(jìn)行標(biāo)定，并利用計(jì)算機(jī)控制左右兩相 機(jī)同步采集測(cè)量圖像，并通過(guò)圖像處理技術(shù)提取光學(xué)祀標(biāo)的圖像坐標(biāo)； 步驟2. 1、利用相機(jī)測(cè)量系統(tǒng)標(biāo)定技術(shù)標(biāo)定出左右兩臺(tái)測(cè)量相機(jī)的內(nèi)參數(shù)和外參數(shù)； 步驟2. 2、通過(guò)同步控制器控制兩臺(tái)測(cè)量相機(jī)W固定測(cè)量頻率采集測(cè)量圖像，并利用質(zhì) 也法提取光學(xué)祀標(biāo)的圖像坐標(biāo)； 步驟2. 3、獲取光學(xué)祀標(biāo)在兩臺(tái)相機(jī)的測(cè)量圖像中對(duì)應(yīng)的質(zhì)也坐標(biāo)后，對(duì)成像鏡頭的崎 變進(jìn)行校正補(bǔ)償； 步驟2. 4、完成崎變校正后，獲得光學(xué)祀標(biāo)在測(cè)量坐標(biāo)系下的H維坐標(biāo)； 步驟3、利用步驟1、步驟2所獲得的光學(xué)祀標(biāo)在被測(cè)物坐標(biāo)系下的H維坐標(biāo)和在測(cè)量 坐標(biāo)系下的H維坐標(biāo)，獲得被測(cè)物坐標(biāo)系相對(duì)測(cè)量坐標(biāo)系的旋轉(zhuǎn)矩陣，從而獲得被測(cè)物的 H維空間姿態(tài)角。
2. 根據(jù)權(quán)利要求1所述的一種不規(guī)則外形物體空間姿態(tài)動(dòng)態(tài)測(cè)量方法，其特征在于： 所述的步驟1中利用激光跟蹤儀對(duì)光學(xué)祀標(biāo)進(jìn)行全局校準(zhǔn)的具體步驟為： 利用激光跟蹤儀測(cè)量H個(gè)光學(xué)祀標(biāo)的空間H維坐標(biāo)，并將激光跟蹤儀坐標(biāo)系設(shè)為標(biāo)定 坐標(biāo)系〇E-X義Z。，光學(xué)祀標(biāo)在標(biāo)定坐標(biāo)系下的H維坐標(biāo)為（X。（i)，Y。（i)，Z。（i))，（i=l， 2,3);利用激光跟蹤儀測(cè)量被測(cè)物體的坐標(biāo)系基準(zhǔn)，建立被測(cè)物坐標(biāo)系與標(biāo)定坐標(biāo)系的轉(zhuǎn) 換關(guān)系，將光學(xué)祀標(biāo)在標(biāo)定坐標(biāo)系下的H維坐標(biāo)轉(zhuǎn)換為在被測(cè)物坐標(biāo)系下的H維坐標(biāo)（Xt (i)，Yt (i)，Zt (i))，（i=l，2,3)〇
3. 根據(jù)權(quán)利要求1所述的一種不規(guī)則外形物體空間姿態(tài)動(dòng)態(tài)測(cè)量方法，其特征在于： 所述的步驟2. 1中標(biāo)定左右兩臺(tái)測(cè)量相機(jī)的內(nèi)參數(shù)和外參數(shù)分別為： 利用平面標(biāo)定法標(biāo)定出左右兩臺(tái)測(cè)量相機(jī)的內(nèi)參數(shù)（(；<"，心"，f/"，k/"，P 1("，口2山）（i=l，2 ;1代表左相機(jī)，2代表右相機(jī)）和外參數(shù)R、T，其中，C/)，Cy("為i相機(jī)的 鏡頭主點(diǎn)位置車W，fyW為i相機(jī)的有效焦距；k/"，為i相機(jī)的鏡頭徑向崎變系數(shù). Pi")，P2")為i相機(jī)鏡頭切向崎變系數(shù)；R為旋轉(zhuǎn)矩陣；T為平移矩陣。
4. 根據(jù)權(quán)利要求1述的一種不規(guī)則外形物體空間姿態(tài)動(dòng)態(tài)測(cè)量方法，其特征在于：所 述的步驟2. 2的具體步驟為： 通過(guò)同步控制器控制兩臺(tái)測(cè)量相機(jī)W固定測(cè)量頻率采集測(cè)量圖像，通過(guò)直方圖分析、 闊值分割圖像處理手段濾除圖像背景噪聲，獲得僅含有光學(xué)祀標(biāo)的理想圖像，利用質(zhì)也法 提取光學(xué)祀標(biāo)的圖像坐標(biāo)（XiW，yi (")和（x/"，yrW) (i = l，2,3)，其中，x/"和y/"為左相 機(jī)測(cè)量的第i個(gè)光學(xué)祀標(biāo)的圖像坐標(biāo)，x/"，y/"為右相機(jī)測(cè)量的第i個(gè)光學(xué)祀標(biāo)的圖像坐 標(biāo)。
5. 根據(jù)權(quán)利要求1述的一種不規(guī)則外形物體空間姿態(tài)動(dòng)態(tài)測(cè)量方法，其特征在于：所 述的步驟2. 3的具體步驟為： 獲取光學(xué)祀標(biāo)在兩臺(tái)相機(jī)的測(cè)量圖像中對(duì)應(yīng)的質(zhì)也坐標(biāo)后，對(duì)成像鏡頭的崎變進(jìn)行校 正補(bǔ)償，校正模型為：
式中；X。、y。為目標(biāo)質(zhì)也校正后的圖像坐標(biāo)，X、y為目標(biāo)質(zhì)也校正前的圖像坐標(biāo)，5 X、 S y為鏡頭崎變；V k2為鏡頭徑向崎變系數(shù)，Pi、P2為鏡頭切向崎變系數(shù)，Cy、Cy為鏡頭主點(diǎn) 位置，此6項(xiàng)參數(shù)通過(guò)相機(jī)系統(tǒng)標(biāo)定后為已知量。
6. 根據(jù)權(quán)利要求1述的一種不規(guī)則外形物體空間姿態(tài)動(dòng)態(tài)測(cè)量方法，其特征在于：所 述的步驟2. 4的具體步驟為： 完成崎變校正后，基于下式模型求解祀標(biāo)在測(cè)量坐標(biāo)系〇m-XmYmZm下的H維坐標(biāo)：
式中；Xm、Ym、Zm為光學(xué)祀標(biāo)在測(cè)量坐標(biāo)系下的立維坐標(biāo)，Xui、yul、Xut、yur為校正后的左、 右圖像坐標(biāo)，fyl、fyl、fyt、fyt分別表示左、右相機(jī)的有效焦距；矩陣R和T為：
fxl、fyl、f"、fyr和R、T通過(guò)標(biāo)定后為已知量。
7. 根據(jù)權(quán)利要求1述的一種不規(guī)則外形物體空間姿態(tài)動(dòng)態(tài)測(cè)量方法，其特征在于：所 述的步驟3的具體步驟為： 步驟3. 1、利用步驟2獲得光學(xué)祀標(biāo)在測(cè)量坐標(biāo)系下的H維坐標(biāo)（XmW，YmW，ZmW) (i=l，2, 3)后，結(jié)合步驟1獲得的光學(xué)祀標(biāo)在被測(cè)物坐標(biāo)系下的H維坐標(biāo)a/"，Y/"，z/") (i=l，2, 3)，求解被測(cè)物坐標(biāo)系相對(duì)測(cè)量坐標(biāo)系的旋轉(zhuǎn)矩陣； 考慮坐標(biāo)系的平移，將坐標(biāo)系原點(diǎn)由點(diǎn)Pi平移到P2點(diǎn)，有關(guān)系式：
考慮坐標(biāo)系的旋轉(zhuǎn)，坐標(biāo)系的旋轉(zhuǎn)可通過(guò)旋轉(zhuǎn)矩陣R描述，繞X、Y、Z軸的旋轉(zhuǎn)矩陣為：


光學(xué)祀標(biāo)在被測(cè)物坐標(biāo)系與測(cè)量坐標(biāo)系中H維坐標(biāo)有如下關(guān)系： 口2 =化 l+t 通過(guò)3個(gè)光學(xué)祀標(biāo)，通過(guò)去中也化處理，得到只含旋轉(zhuǎn)分量的坐標(biāo)，列出如下參數(shù)方 程，通過(guò)優(yōu)化迭代可解算出旋轉(zhuǎn)矩陣R ;
上述方程組中，rl，…，r9為旋轉(zhuǎn)矩陣元素。X。。y。。Zm為目標(biāo)初始點(diǎn)第i點(diǎn)去重也坐 標(biāo)，Xu, y^，Zii為目標(biāo)運(yùn)動(dòng)后對(duì)應(yīng)第i點(diǎn)去重也坐標(biāo)； 步驟3. 2、加入約束條件，獲得被測(cè)物是H維空間姿態(tài)角； 加入如下約束：
優(yōu)化迭代得到旋轉(zhuǎn)矩陣R后，可反求出物體繞H軸的旋轉(zhuǎn)角：


其中，a、目和y為被測(cè)物的S維空間姿態(tài)角，即俯仰角、滾轉(zhuǎn)角和偏航角。
【文檔編號(hào)】G01C1/00GK104422425SQ201310377247
【公開日】2015年3月18日 申請(qǐng)日期:2013年8月27日 優(yōu)先權(quán)日:2013年8月27日
【發(fā)明者】郭磊, 梁雅軍, 劉柯, 孫增玉, 宋金城, 陳曉暉 申請(qǐng)人:北京航天計(jì)量測(cè)試技術(shù)研究所, 中國(guó)運(yùn)載火箭技術(shù)研究院