專(zhuān)利名稱：一種實(shí)時(shí)測(cè)量運(yùn)動(dòng)體位移姿態(tài)角的裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種實(shí)時(shí)測(cè)量運(yùn)動(dòng)物體位移和姿態(tài)角的裝置。由于許多運(yùn)動(dòng)物體的運(yùn)動(dòng)規(guī)律不確定，所處的環(huán)境復(fù)雜，自身負(fù)載能力有限，無(wú)法直接進(jìn)行接觸測(cè)量。選種情況下目前，主要采用機(jī)械掃描式和固體攝像式兩種測(cè)量裝置對(duì)運(yùn)動(dòng)物體的位移和姿態(tài)角進(jìn)行測(cè)量。機(jī)械掃描式測(cè)量裝置主要由三部分組成，光源部分、機(jī)械掃描部分和信號(hào)處理部分，光源部分主要包括控制電路和發(fā)光元件，機(jī)械掃描部分主要包括棱鏡、電機(jī)和光電接收元件，信號(hào)處理部分主要由濾波電路、放大電路和單片機(jī)組成，其工作原理是將一組或多組光源固定在被測(cè)物體上，然后將至少兩套機(jī)械掃描裝置放置在距光源一定的距離，成一定的角度布置，控制光源發(fā)出頻率相同而相位不同的光脈沖，電機(jī)驅(qū)動(dòng)機(jī)械掃描裝置轉(zhuǎn)動(dòng)，光電接收器件將光信號(hào)轉(zhuǎn)換成電信號(hào)進(jìn)到信息處理單元進(jìn)行處理、解算，從而求出光源在空間的一維或二維坐標(biāo)，并由此求出物體的兩個(gè)姿態(tài)角。這種測(cè)量裝置由于受機(jī)械掃描裝置限制，實(shí)時(shí)性較差，測(cè)量精度低，且由于有電機(jī)存在，整個(gè)測(cè)量系統(tǒng)的結(jié)構(gòu)重量較重，所以在實(shí)際使用中受到了限制；固體攝像式測(cè)量裝置由光源、固體攝像機(jī)和信號(hào)處理部分組成，它也是將一組或多組光源固定在被測(cè)物體上，固體攝像機(jī)作為測(cè)量元件，將光信號(hào)變成電信號(hào)，再由信息處理部分進(jìn)行象素處理、運(yùn)算，從而求出光源坐標(biāo)，再由光源坐標(biāo)求出被測(cè)物體的位移和姿態(tài)角，這種測(cè)量裝置要處理的信息量大，致使實(shí)時(shí)性比較差，測(cè)量精度由固體攝像機(jī)分辨率所限制，所以精度較低，而且攝像機(jī)本身較重，使得重量無(wú)法降低。
本發(fā)明的目的在于克服已有技術(shù)的不足，提供一種實(shí)時(shí)性好、測(cè)量精度高、抗干擾能力強(qiáng)，且結(jié)構(gòu)重量輕的實(shí)時(shí)測(cè)量運(yùn)動(dòng)物體的位移和姿態(tài)角的裝置。
本發(fā)明的目的是通過(guò)以下措施實(shí)現(xiàn)的一種實(shí)時(shí)測(cè)量運(yùn)動(dòng)物體位移和姿態(tài)角的裝置包括一組或多組光源及光源驅(qū)動(dòng)器、一組或多組測(cè)量單元和一個(gè)信息處理單元，其特點(diǎn)在于上述的測(cè)量單元由選擇光束或光面的裁光板和光照位移傳感器組成，上述的裁光板置于上述光源和上述光照位移傳感器之間，并與上述的光照位移傳感器平行放置。光源驅(qū)動(dòng)器控制光源發(fā)出光脈沖，由光照位移傳感器對(duì)透過(guò)裁光板的光照位移進(jìn)行測(cè)量，信息處理單元根據(jù)測(cè)得的光照位移量確定不同的光面方程，求解相應(yīng)的發(fā)光點(diǎn)的坐標(biāo)，再由發(fā)光點(diǎn)的坐標(biāo)求解運(yùn)動(dòng)物體的位移和姿態(tài)角。
本發(fā)明與現(xiàn)有技術(shù)相比具有如下優(yōu)點(diǎn)1.由于采用裁光板和光照位移傳感器作為測(cè)量元件，使得本發(fā)明可以使用紅外光源作為光源，因此該裝置對(duì)周?chē)碾姶怒h(huán)境和照明環(huán)境干擾比較小，抗干擾能力也較強(qiáng)。
2.由于光照位移傳感器的分辨率比較高，一般可這幾微米量級(jí)，通過(guò)它可以取得高精度的光源坐標(biāo)，進(jìn)而獲得高的位移和姿態(tài)角精度，因此本發(fā)明測(cè)量精度高。
3.由于本發(fā)明的測(cè)量范圍可以通過(guò)縮短裁光板與光照位移傳感器之間的距離而擴(kuò)大，而且光照位移傳感器的分辨率有較大的余度，因此本發(fā)明可以在同樣精度條件下比機(jī)械掃描測(cè)量裝置具有更大的測(cè)量范圍。
4.由于本發(fā)明采用光照位移傳感器對(duì)光照位移進(jìn)行直接測(cè)量，沒(méi)有機(jī)械環(huán)節(jié)和圖像處理環(huán)節(jié)，所以本發(fā)明采樣速度和數(shù)據(jù)處理的速度都比較高，比機(jī)械掃描式測(cè)量裝置和固體攝像式測(cè)量的實(shí)時(shí)性都好，機(jī)械掃描式和固體攝像式的測(cè)量延遲一般大于30毫秒，本系統(tǒng)可使延遲時(shí)間控制在10毫秒以內(nèi)。
5.由于裁光板和光照位移傳感器都結(jié)構(gòu)簡(jiǎn)單，質(zhì)量較小，所以本發(fā)明總的結(jié)構(gòu)重量較輕。
下面結(jié)合附圖及實(shí)施例對(duì)本發(fā)明進(jìn)一步詳細(xì)說(shuō)明。


圖1為本發(fā)明的組成原理框圖；圖2為本發(fā)明測(cè)量單元一種結(jié)構(gòu)簡(jiǎn)圖；圖3為本發(fā)明測(cè)量單元另一種結(jié)構(gòu)簡(jiǎn)圖；圖4為本發(fā)明信息處理單元原理框圖；圖5為本發(fā)明信息處理單元電原理圖；圖6為本發(fā)明解算光源坐標(biāo)示意圖；圖7為本發(fā)明實(shí)時(shí)測(cè)量程序框圖。
參見(jiàn)附圖1、2、3，固定在被測(cè)物體上的光源1采用紅外點(diǎn)光源，幾個(gè)發(fā)光點(diǎn)合稱為一組，根據(jù)測(cè)量范圍的大小可以選用多組光源，本實(shí)施例采用3個(gè)發(fā)光點(diǎn)為一組的光源，光源1隨著被測(cè)物體在裁光板3即A′B′C′D′正前30公分處直徑為半米的球內(nèi)，即測(cè)量范圍內(nèi)運(yùn)動(dòng)。光源驅(qū)動(dòng)器2控制光源1使其發(fā)出頻率相同而相位不同的光脈沖，裁光板3和光照位移傳感器4組成測(cè)量單元，并由光照位移傳感器4對(duì)透過(guò)裁光板3的光照位移進(jìn)行測(cè)量。裁光板3的作用是完成對(duì)光束或光面的選取，光照位移傳感器4的作用是測(cè)定光照位移量，它是一種半導(dǎo)體器件，在接收光照信號(hào)后，兩極輸出電流，兩極電流的偏差與光照位移成線性關(guān)系，與光強(qiáng)無(wú)關(guān)。裁光板3有兩種設(shè)計(jì)結(jié)構(gòu)，第一種是在薄的透光材料下面鍍一層遮光膜，然后在遮光膜上刻畫(huà)三條不全平行的透光縫6，參見(jiàn)附圖2，透光縫6的長(zhǎng)度根據(jù)測(cè)量范圍大小來(lái)選擇，如果測(cè)量范圍大，需要光源投射的角度就大，那么透光縫6的長(zhǎng)度就要長(zhǎng)一些，一般取3-50毫米，透光縫6寬度可根據(jù)光強(qiáng)和精度選取，如果測(cè)量精度高，光強(qiáng)度高，那么透光縫6就要窄一些，一般在100-500微米之間選取，對(duì)于圖2的裁光板需要采用一維光照位移傳感器4來(lái)測(cè)量，該一維傳感器4與裁光板平行放置，其敏感面對(duì)著裁光板，當(dāng)一維傳感器4的敏感面大小確定時(shí)，兩者之間的距離00′與測(cè)量范圍成反比，當(dāng)測(cè)量范圍確定時(shí)，兩者之間的距離00′與光照位移傳感器4的敏感面的長(zhǎng)度成正比，即根據(jù)不同的測(cè)量范圍要求和光照位移傳感器敏感面的尺寸來(lái)選取裁光板與傳感器之間的距離。當(dāng)光源1發(fā)光時(shí)，三個(gè)透光縫6各取某一光面，投射于相應(yīng)一維光照位移傳感器敏感面上，一維光照位移傳感器4即可測(cè)出沿相應(yīng)方向上的光照位移。裁光板3的另一設(shè)計(jì)結(jié)構(gòu)是在薄的透光材料下面鍍一層遮光膜，在遮光膜上加工三個(gè)微型透光孔7，參見(jiàn)附圖3，微型透光孔7的直徑根據(jù)光強(qiáng)和精度要求在100-500微米中間選取，對(duì)于圖2的裁光板需要采用二維光照位移傳感器4′來(lái)測(cè)量，該二維光照位移傳感器4′與裁光板也平行放置，其敏感面也需對(duì)著裁光板，兩者之間的距離也是與測(cè)量范圍成反比，與該二維光照位移傳感器4′的敏感面尺寸成正比。當(dāng)光源1發(fā)光時(shí)，微型透光孔7選取不同的光束線，投射于二維光照位移傳感器4′的敏感面上，該二維光照位移傳感器4′測(cè)出光束在敏感面上的光照位移量，之后再由信息處理單元5根據(jù)光照位移量確定三個(gè)不同的光面方程，再由光面方程實(shí)時(shí)解算出每個(gè)發(fā)光點(diǎn)的坐標(biāo)，根據(jù)發(fā)光點(diǎn)的坐標(biāo)再求出位移和姿態(tài)角。
參見(jiàn)附圖4，信息處理單元5由信號(hào)提取電路和完成實(shí)時(shí)解算與控制系統(tǒng)的單片機(jī)系統(tǒng)組成。信號(hào)提取電路由放大電路51、求差電路52、求和電路53、求比電路54、采樣保持電路55、A/D轉(zhuǎn)換電路56、實(shí)時(shí)解算控制部件單片機(jī)系統(tǒng)57組成。光照位移傳感器4輸出的光照位移量經(jīng)放大電路51放大后分兩路，一路進(jìn)入求差電路52，對(duì)放大了的光照位移信號(hào)進(jìn)行求差運(yùn)算，另一路進(jìn)入求和電路53，對(duì)放大了的光照位移信號(hào)進(jìn)行求和運(yùn)算，求差電路52和求和電路53的輸出經(jīng)求比電路54進(jìn)行求比運(yùn)算后，使光照位移信號(hào)轉(zhuǎn)化成具有一定精度的可分辨的模擬信號(hào)，該模擬信號(hào)由采樣保持電路55、A/D轉(zhuǎn)換電路56轉(zhuǎn)換成數(shù)字信號(hào)后，送到實(shí)時(shí)解算控制單片機(jī)系統(tǒng)57中處理，完成實(shí)時(shí)計(jì)算，單片機(jī)系統(tǒng)57同時(shí)還控制光源驅(qū)動(dòng)器2驅(qū)動(dòng)光源1。
參見(jiàn)附圖5，三個(gè)光照位移傳感器4，測(cè)量的光照位移量首先經(jīng)0P1-0P6運(yùn)算放大對(duì)其進(jìn)行放大，再由OP7、0P9、0P11對(duì)信號(hào)求和，由0P8、OP10、0P12對(duì)信號(hào)求差，AD734、AD7342、AD7343對(duì)求出的差與和信號(hào)求比，LP3981-LE3983對(duì)三個(gè)通道的光照位移信號(hào)實(shí)時(shí)進(jìn)行采樣保持，AD5741-AD5743則對(duì)相應(yīng)的LF3981-LF3983輸出的信號(hào)進(jìn)行模數(shù)轉(zhuǎn)換，轉(zhuǎn)換完成后由8255統(tǒng)一輸入單片機(jī)系統(tǒng)8098中，由8098、6264、2764、8255、74LS133、74LS138一起構(gòu)成實(shí)時(shí)運(yùn)算和控制模塊。光源驅(qū)動(dòng)電路2由MC1413和相應(yīng)的發(fā)光管組成，最終的計(jì)算結(jié)果也由8255通過(guò)PA口輸出。
參見(jiàn)附圖6，解算光源坐標(biāo)的具體過(guò)程如下建立如圖6所示的直角坐標(biāo)系，假設(shè)采用加工有透光縫6的栽光板，且設(shè)w為透光縫6的長(zhǎng)度，l表示坐標(biāo)原點(diǎn)到三個(gè)透光縫6的距離，h表示透光縫6到光照位移傳感器4敏感面的距離，p1、p2、p3為三個(gè)光照位移傳感器測(cè)得的光照位移量，則有如下三個(gè)點(diǎn)同處于一個(gè)光面AP上，可表示為(l，w，h)，(l，-w，h)，(l+p1，0，0)；同理在BP平面上也有三個(gè)點(diǎn)分別可表示為(w，l，h)，(-w，l，h)，(0，l+p2，0)；在CP平面上有三個(gè)點(diǎn)分別可表示為(w，-l，h)，(-w，-l，h)，(O，-l+p3，0)，因?yàn)闃?biāo)準(zhǔn)的平面方程為 AX+BY+CZ＝D將AP平面上的三個(gè)點(diǎn)的坐標(biāo)分別代入其中得 解得B＝0，C＝p1Ah，D＝(l+p1)A故，X+p1Z/h＝l+P1 (1)同理BP平面有Y+p2Z/h＝l+p2 (2)CP平面有
Y+p1Z/h＝-l+p3 (3)解(1)、(2)、(3)組成的聯(lián)立方程得 (4)就是光源坐標(biāo)算法表達(dá)式，按照該表達(dá)式可以求出一組中三個(gè)不同的光源坐標(biāo)。
根據(jù)上述三個(gè)光源坐標(biāo)即可求出運(yùn)動(dòng)物體的位移和姿態(tài)角，具體過(guò)程如下假設(shè)上述一組光源的三個(gè)發(fā)光點(diǎn)，相對(duì)距離為10公分，解出的坐標(biāo)為Q1(x1，y1，z1)、Q2(x2，y2，z2)和Q3(x3，y3，z3)，設(shè)01為Q1Q2的中點(diǎn)，則有O1((x1+x2)/2，(y1+y2)/2，(z1+z2)/2)則向量Q1Q2的表達(dá)式為(x2-x1，y2-y1，z2-z1)，向量O1Q3的表達(dá)式為(x3-(x1+x2)/2，y3-(y1+y2)，z3-(z1+z2)/2)，由向量投影的基本原理得Q1Q2沿Z軸的投影為|Q1Q2|z＝z2-z1所以，運(yùn)動(dòng)物體沿水平方向上的轉(zhuǎn)動(dòng)角度為DA＝arcsin(|Q1Q2|z/|Q1Q2|)-a0＝arcsin((z2-z1)/10)-a0同理可得|Q1Q3|z＝z3(z1+z2)/2，
|Q1Q3|x＝x3-(x2-x1)/2DB＝arcsin(|Q1Q|z/Q1Q3|)-b0＝arcsin((z3-(z2+z1)/2/0.866×10)-b0DR＝arcsin(|Q1Q3|x/|Q1Q3|)-r0＝arcsin((x3-(x2+x1)/2)/0.866×10)-r0則DA、DB和DR就是所求的運(yùn)動(dòng)物體的姿態(tài)角，a0、b0和r0為初始姿態(tài)角。
對(duì)于位移的求解過(guò)程如下因?yàn)镼1Q2＝(x2-x1，y2-y1，z2-z1)，Q1Q3＝(x3-x1，y3-y1，z3-z1)，根據(jù)向量積的運(yùn)算可知其法向向量n為ijkx2-x1y2-y1z2-z1x3-x1y3-y1z3-z1]]>＝((y2-y1)(z3-z1)-(y3-y1)(z2-z1)i((z2-z1)(z3-z1)-(x3-x1)(z2-z1)j((x2-x1)(y3-y1)-(x3-x1)(y2-y1)k＝Nxi+Nyj+Nzk同樣由向量積的運(yùn)算法則可知它的模為0.866×102由此可知運(yùn)動(dòng)物體的位移DX、DY和DZ分別為DZ＝(x1+x2+x3)/3-m×Nx/0.8660×102-x0DY＝(y1+y2+y3)/3-m×Ny/0.866×102-y0DX＝(z1+z2+z3)/3-m×Nz/0.866×102-z0這就是所求的運(yùn)動(dòng)物體位移表達(dá)式。上式中的x0、y0、z0為初始X、Y、Z值，m為運(yùn)動(dòng)物體中心到光源中心的距離。
本發(fā)明是一種實(shí)時(shí)測(cè)量裝置，在裝置工作時(shí)，可以對(duì)位移和姿態(tài)角進(jìn)行連續(xù)實(shí)時(shí)的檢測(cè)，實(shí)際工作程序參見(jiàn)附圖6，首先進(jìn)行系統(tǒng)初始化，對(duì)于單片機(jī)和環(huán)境參數(shù)進(jìn)行設(shè)置，接著單片機(jī)系統(tǒng)控制光源驅(qū)動(dòng)器驅(qū)動(dòng)光源發(fā)光，光脈沖形成的光照位移信號(hào)由光照位移傳感器接收，單片控制開(kāi)始采樣，采樣完成后開(kāi)始保持，并啟動(dòng)AD轉(zhuǎn)換電路完成轉(zhuǎn)換，控制8255開(kāi)始讀取數(shù)據(jù)并存貯，由8098CPU進(jìn)行數(shù)據(jù)預(yù)處理，按上面所述方法求解光源坐標(biāo)，進(jìn)一步求出被測(cè)對(duì)象的位移和姿態(tài)角，一面將位移和姿態(tài)角輸出，一面根據(jù)姿態(tài)角的范圍確定下一次應(yīng)對(duì)哪一組光源進(jìn)行測(cè)量，確定后即控制光源發(fā)光，又重復(fù)上述過(guò)程，這樣即可反復(fù)進(jìn)行實(shí)時(shí)測(cè)量。
本發(fā)明因?yàn)榫哂猩鲜鰞?yōu)點(diǎn)，在飛行模擬器、飛機(jī)或坦克頭盔瞄準(zhǔn)具上擁有廣泛的應(yīng)用前景。下面以飛行模擬器頭位跟蹤系統(tǒng)中的應(yīng)用情況為例說(shuō)明其使用情況。將光源固定在駕駛員頭盔上，將測(cè)量單元放在座艙上光源可照到的一側(cè)，信息處理單元放在座艙附近，設(shè)置好初始參數(shù)，啟動(dòng)本裝置工作，則本裝置可實(shí)時(shí)高速(延遲不超過(guò)10毫秒)將駕駛員的頭部位移和姿態(tài)角送給主控計(jì)算機(jī)，用此信號(hào)可對(duì)視景進(jìn)行切換控制和對(duì)要攻擊的目標(biāo)進(jìn)行跟蹤攻擊。
權(quán)利要求
1.一種實(shí)時(shí)測(cè)量運(yùn)動(dòng)物體位移和姿態(tài)角的裝置，它包括一組或多組光源及光源驅(qū)動(dòng)器、一組或多組測(cè)量單元和一個(gè)信息處理單元，其特征在于上述的測(cè)量單元由選擇光束或光面的裁光板和光照位移傳感器組成，上述的裁光板置于上述光源和上述光照位移傳感器之間，并與光照位移傳感器平行放置。
2.根據(jù)權(quán)利要求1所述的測(cè)量裝置，其特征在于上述裁光板由薄的透光材料及在其下面鍍的一層遮光膜組成，并在遮光膜上記刻畫(huà)三條不全平行的透光縫或在遮光膜上加工出三個(gè)微型透光孔。
3.根據(jù)權(quán)利要求1所述的測(cè)量裝置，其特征在于光照位移傳感器敏感面長(zhǎng)度確定時(shí)，裁光板與光照位移傳感器之間的距離與測(cè)量范圍成反比，測(cè)量范圍確定時(shí)，裁光板與光照位移傳感器之間的距離與光照位移傳感器敏感面的長(zhǎng)度成正比。
4.根據(jù)權(quán)利要求1所述的測(cè)量裝置，其特征在于信號(hào)處理單元由信號(hào)提取電路和實(shí)時(shí)解算和控制系統(tǒng)的單片機(jī)系統(tǒng)組成，信號(hào)提取電路由放大電路、求和電路、求差電路、求比電路、采樣保持及A/D轉(zhuǎn)換電路組成，光照位移傳感器測(cè)得的位移量經(jīng)放大電路后分兩路，一路進(jìn)入求和電路，另一路進(jìn)入求差電路，求和電路和求差電路的輸出進(jìn)入求比電路，求比電路的輸出經(jīng)采樣保持及A/D電路后進(jìn)入單片機(jī)系統(tǒng)。
全文摘要
一種實(shí)時(shí)測(cè)量運(yùn)動(dòng)物體位移和姿態(tài)角的裝置，包括一組或多組光源及光源驅(qū)動(dòng)器、一組或多組測(cè)量單元和一個(gè)信息處理單元，其特點(diǎn)在于測(cè)量單元由選擇光束或光面的裁光板和光照位移傳感器組成，利用光照位移傳感器對(duì)透過(guò)裁光板的光照位移量進(jìn)行測(cè)量，信息處理單元根據(jù)光照位移量求解不同的光面方程，算出光源在空間的相對(duì)坐標(biāo)，進(jìn)而求出運(yùn)動(dòng)物體三個(gè)方向的位移量和三個(gè)姿態(tài)角。本發(fā)明具有實(shí)時(shí)性好、測(cè)量范圍大、測(cè)量精度高、抗干擾能力強(qiáng)和結(jié)構(gòu)重量輕的優(yōu)點(diǎn)。
文檔編號(hào)G01B11/02GK1148161SQ9610112
公開(kāi)日1997年4月23日 申請(qǐng)日期1996年2月12日 優(yōu)先權(quán)日1996年2月12日
發(fā)明者鄧先法, 潭澤勝, 劉莉萍, 楊慶峰, 熊笑非, 郭曉輪, 溫志誠(chéng), 宋焱 申請(qǐng)人:中國(guó)人民解放軍空軍第八研究所