專利名稱：一種三維位姿探測裝置及其測量方法
技術領域：
本發(fā)明涉及一種三維位姿探測裝置及其測量方法。
背景技術：
空間三維位姿探測在現(xiàn)代化作業(yè)、空間探測和軍事等領域有著廣泛的應用。為了感知作業(yè)方所處的環(huán)境，我們需要采用照相機或攝像機，為了采取進一步的操作或完成相互協(xié)調(diào)動作，我們需要精確地知道作業(yè)方的位置和姿態(tài)，這就需要采用至少3個成像設備以確定其精確的空間位置和姿態(tài)。這樣的結果一是大大增加了三維位姿探測機構的復雜性，二是使得整體的控制方法變得十分繁雜，容錯性變差。空間三維定位發(fā)展較早的是立體視覺理論，應用較多的是雙目立體視覺，其基本原理類似于人的視力，通過頻繁的信號交替可以精確完成給定的動作，這里的問題有兩個，一是對應雙目需要配置兩個攝像機構，二是雙目無法精確定位距離遠近。因此，很多研究機構致力于開發(fā)對應的算法，如一種空間目標三維位姿視覺測量方法(國別中國，公開號101464134A，
公開日期2009年6月24日)等。

發(fā)明內(nèi)容
發(fā)明目的針對上述問題，本發(fā)明的目的是提供一種結構簡單、控制容易的探測對象空間位置和姿態(tài)的三維位姿探測裝置；本發(fā)明的另一目的是提供使用這種三維位姿探測裝置進行測量的方法，完成空間探測功能需要和精準的測量。技術方案一種三維位姿探測裝置，包括二維位置敏感探測器、凸透鏡、發(fā)射板，所述發(fā)射板上的點光源發(fā)出光線通過所述凸透鏡在所述二維位置敏感探測器的光敏面上成像為清晰點，所述發(fā)射板在中心和四角共設置五個所述點光源，且相鄰兩個角落的所述點光源的距離為20mm。所述點光源采用發(fā)光二極管。點光源發(fā)出的光線作為二維位置敏感探測器檢測的信號，要求該點光源的選取應直徑要盡可能小且發(fā)射角要盡量大，這樣當不可避免的出現(xiàn)近距離檢測時以及受到二維位置敏感探測器自身檢測范圍的限制時，保證二維位置敏感探測器能夠檢測到點光源的位置。所述光敏面為正方形，所述二維位置敏感探測器的視角為30°。對二維位置敏感探測器的要求是視角越大，能感知到的最小距離越小，對齊精確度就會越高。但是視角不能無限增大，因為視角越大，在最小距離時發(fā)射板上的點光源陣列在二維位置敏感探測器上的成像就會越小，分辨率和精確度都會受到影響，因此取視角為30°左右。同時，由于在二維位置敏感探測器上X和Y方向等價，因此盡量選擇光敏面為正方形的二維位置敏感探測器。上述一種三維位姿探測裝置的測量方法，包括以下步驟步驟(I):測量空間距離將所述二維位置敏感探測器的所述光敏面放置于所述凸透鏡的焦平面上，且所述光敏面的中心與所述凸透鏡的焦點重合，所述凸透鏡的位置為F，所述凸透鏡和所述二維位置敏感探測器構成接收板；所述發(fā)射板四角的所述點光源A’、B’、C’、D’ 和中心的所述點光源 0’，使 0’ A’ = 0’ B’ = 0’ C’ = 0’ D’，且 A’、B’、C’、D’ 中相對的兩個分別與所述發(fā)射板的X’軸、Y’軸重合;A、B、C、D為A’、B’、C’、D’在所述光敏面上對應的成像點；經(jīng)調(diào)整使所述發(fā)射板的Z’軸與所述接收板的Z軸對正時，得到所述接收板與所述發(fā)射板之間的距離0’ F ；步驟(2):對齊接收板與發(fā)射板信號捕捉所述發(fā)射板沿其X軸、Y軸、Z軸分別移動、轉(zhuǎn)動，直到接收到所述發(fā)射板的所述點光源的成像；接收板與發(fā)射板的對齊按給定的步長和運動次序移動所述接收板至其與所述發(fā)射板正好對齊，此時所述接收板與所述發(fā)射板姿態(tài)一致，由所述接收板姿態(tài)得知所述發(fā)射板姿態(tài)；對齊判據(jù)為AC 丄 BD, 0. 5 (xA + xcX 0. lmm、0. 5 (yA + yc) ^ 0. 1mm, 0. 5 (xB +xD) ( 0. lmm、0. 5 (yB + yD) ( 0. 1mm,其中 0.1mm 為要求的誤差精度。所述發(fā)射板上的五個所述點光源通過單片機控制發(fā)光順序、發(fā)光持續(xù)時間、發(fā)光間隔時間。有益效果與現(xiàn)有技術相比，本發(fā)明的優(yōu)點是將二維位置敏感探測器添加光學裝置進行改裝，用于三維空間的位姿檢測，并通過發(fā)射板的布置，結合測量方法協(xié)調(diào)工作，完成空間探測和精準測量；探測裝置結構簡單，控制容易，測量精確度高。


圖1為本發(fā)明三維位姿探測裝置的結構示意圖；圖2為發(fā)射板上點光源的分布位置圖；圖3為本發(fā)明測量方法原理圖；圖4為單片機控制的發(fā)射板部分的流程圖；圖5為單片機控制的接收板部分的流程圖。
具體實施例方式下面結合附圖和具體實施例，進一步闡明本發(fā)明，應理解這些實施例僅用于說明本發(fā)明而不用于限制本發(fā)明的范圍，在閱讀了本發(fā)明之后，本領域技術人員對本發(fā)明的各種等價形式的修改均落于本申請所附權利要求所限定的范圍。本發(fā)明三維位姿探測裝置是對二維位置敏感探測器作出改裝，運用于三維空間進行位姿探測。如附圖1、2所示，一種三維位姿探測裝置，包括二維位置敏感探測器1、凸透鏡2、發(fā)射板3，發(fā)射板3上的點光源4發(fā)出光線通過凸透鏡2在二維位置敏感探測器I的光敏面5上成像為清晰點。發(fā)射板3上設置有五個點光源4，分布在發(fā)射板3的中心和四角，這樣的布置是為了安排時序以實現(xiàn)位姿感知的控制測量，其中相鄰兩個角落的點光源4的距離為20mm。點光源4采用發(fā)光二極管， 點光源4的選取原則是應直徑要盡可能小且發(fā)射角要盡量大，這樣當不可避免的出現(xiàn)近距離檢測時以及受到二維位置敏感探測器I自身檢測范圍的限制時，保證二維位置敏感探測器1能夠檢測到點光源4的位置。
由于二維位置敏感探測器I檢測的信號是來自點光源4發(fā)出的光線，盡管已經(jīng)選取了直徑比較小的發(fā)光二極管來模擬點光源，但是當二維位置敏感探測器I跟發(fā)光二極管距離較近時，發(fā)光二極管的發(fā)光狀況仍然跟點光源相差甚遠。為了使來自發(fā)光二極管的光線接近于點光源，同時也為了使光強對二維位置敏感探測器I的影響減到最小和擴大二維位置敏感探測器I的檢測距離，采取與照相機鏡頭的工作原理相同的光學裝置改裝二維位置敏感探測器1，二維位置敏感探測器I和凸透鏡2構成新型的三維位姿探測器，能將來自發(fā)光二極管的部分光線會聚成二維位置敏感探測器I的光敏面5上的一點，也就是說，能使每個發(fā)光二極管都在光敏面5上有一個清晰的像點。對二維位置敏感探測器I和凸透鏡2的裝配可以使用固定支架6連接。對二維位置敏感探測器I的要求是視角越大，能感知到的最小距離越小，對齊精確度就會越高。但是視角不能無限增大，因為視角越大，在最小距離時發(fā)射板3上的點光源4陣列在二維位置敏感探測器I上的成像就會越小，分辨率和精確度都會受到影響，因此取視角為30°左右。同時，由于在二維位置敏感探測器I上X和Y方向等價，因此盡量選擇光敏面為正方形的二維位置敏感探測器。附圖3所示為使用本發(fā)明三維位姿探測裝置進行探測的測量方法原理圖，具體的測量方法包括以下步驟步驟(I):測量空間距離將二維位置敏感探測器I的光敏面5放置于凸透鏡2的焦平面上，且光敏面5的中心與凸透鏡2的焦點重合，凸透鏡2的位置為F，凸透鏡2和二維位置敏感探測器I構成接收板；所述 發(fā)射板3四角的點光源4分別標為A’、B’、C’、D’，中心的點光源4標為0’，使0’ A’ = 0’ B’ = 0’ C’ = 0’ D’，且A’ C’與發(fā)射板3的Y’軸重合，B’ D’與發(fā)射板3的X’軸重合'K、B、C、D為A’、B’、C’、D’發(fā)出的光線經(jīng)凸透鏡2聚焦后在光敏面5上對應的成像點。經(jīng)空間幾何投影關系可知，如果發(fā)射板3的Z’軸與接收板的Z軸對正時，可以得到(A’ 0’ /0’ F) = (A0/0F),即0’ F = (A' 0’ .OF/AO)，得到接收板與發(fā)射板3之間的距離0’ F。因此，要測量接收板與發(fā)射板3之間的距離，關鍵問題是要調(diào)整接收板的Z軸與發(fā)射板3的Z’軸，使其精確對準。步驟(2):對齊接收板與發(fā)射板信號捕捉發(fā)射板3在其工作空間內(nèi)沿其X軸、Y軸、Z軸分別移動、轉(zhuǎn)動，直到接收到發(fā)射板3的點光源4的成像，完成信號捕捉過程；接收板與發(fā)射板的對齊按給定的步長和運動次序移動接收板至其與發(fā)射板3正好對齊，對齊判據(jù)為AC 丄 BD，0. 5 (xA + xc) ( 0. lmm、0. 5 (yA + yc) ( 0. 1mm，0. 5 (xB +Xd) ( 0. lmm、0. 5 (yB + yD)彡0. 1mm,其中0.1mm為要求的誤差精度。此時接收板與發(fā)射板3姿態(tài)一致，由接收板自身姿態(tài)可知發(fā)射板3的姿態(tài)，由空間距離的感知可以測得空間相對位置，從而得到發(fā)射板3的空間位姿。因此，可以在被感知物上放置發(fā)射板3來實現(xiàn)被感知。以上探測的測量過程通過單片機來自動控制。發(fā)射板和接收板通過發(fā)光二極管陣列和接收板的二維位置敏感探測器之間的光電感應信號形成閉環(huán)回路，由接收板的處理器對位置偏差進行處理，從而控制接收板按照處理的結果運動，運動的結果會減小位置偏差。發(fā)射板的工作過程如附圖4所示由發(fā)射板的主處理器發(fā)出指令給其控制模塊，該指令包括給定的A/D脈沖頻率、各個發(fā)光二極管的發(fā)光頻率和發(fā)光順序、每個發(fā)光二極管的發(fā)光持續(xù)時間、相鄰發(fā)光二極管的發(fā)光間隔時間；經(jīng)控制模塊處理過的指令直接發(fā)送給發(fā)光二極管的驅(qū)動電路，則發(fā)光二極管按驅(qū)動電路所給的信號不斷閃光。接收板的工作過程如附圖5所示接收板首先按照測量空間距離的算法在其空間中搜尋發(fā)光二極管的光線信號，當找到目標信息時，接收板的二維位置敏感探測器的四個輸出端輸出電平信號到自身的信號處理電路中，經(jīng)過信號處理電路處理過的信號表示處于發(fā)射板上一個發(fā)光二極管在二維位置敏感探測器上對應的一組X、Y值，該值可以確定該發(fā)光二極管對應的成像點在二維位置敏感探測器平面上的位置。發(fā)射板上不同發(fā)光二極管的位置依靠時序來區(qū)分。本發(fā)明的設計實現(xiàn)過程主要如下( I)根據(jù)設定的要求，完成發(fā)射板的組裝；(2)為選定的二維位置敏感探測器安裝光學裝置，使光學裝置的焦平面正好位于二維位置敏感探測器的光敏面上；(3)利用單片機實現(xiàn)發(fā)射板上點光源的時序控制；(4)被探測物上安裝由點光源陣列和控制器組成的發(fā)射板，并在被探測物上安裝由二維位置敏感探測器和光學裝置構成新型的三維位姿探測器；(5)搭建控制系統(tǒng)，由主處理器和底層控制模塊組成，如圖4所示，二維位置敏感探測器信號經(jīng)過信號處理電路和A/D轉(zhuǎn)換器，把二維坐標以數(shù)值形式發(fā)送給主處理器；主處理器根據(jù)接收到的信號值和程序執(zhí)行過程，進一步控制接收板的運動和發(fā)射板的子控制器；( 6 )進行自動化測試。
權利要求
1.一種三維位姿探測裝置，其特征在于包括二維位置敏感探測器(I)、凸透鏡(2)、發(fā)射板(3)，所述發(fā)射板(3)上的點光源(4)發(fā)出光線通過所述凸透鏡(2)在所述二維位置敏感探測器(I)的光敏面(5)上成像為清晰點，所述發(fā)射板(3)在中心和四角共設置五個所述點光源(4)，且相鄰兩個角落的所述點光源(4)的距離為20mm。
2.根據(jù)權利要求1所述的一種三維位姿探測裝置，其特征在于所述點光源(4)采用發(fā)光二極管。
3.根據(jù)權利要求1所述的一種三維位姿探測裝置，其特征在于所述光敏面(5)為正方形。
4.根據(jù)權利要求1所述的一種三維位姿探測裝置，其特征在于所述二維位置敏感探測器(I)的視角為30°。
5.一種權利要求1所述的三維位姿探測裝置的測量方法，其特征在于包括以下步驟 步驟(I):測量空間距離將所述二維位置敏感探測器(I)的所述光敏面(5)放置于所述凸透鏡(2)的焦平面上，且所述光敏面(5)的中心與所述凸透鏡(2)的焦點重合，所述凸透鏡(2)的位置為F，所述凸透鏡(2)和所述二維位置敏感探測器(I)構成接收板；所述發(fā)射板(3)四角的所述點光源(4),、8’、(’、0’和中心的所述點光源(4)0’，使O’A’ =0’B’= 0’C’ =0’0’，且,、8’、(’、0’中相對的兩個分別與所述發(fā)射板(3)的X’軸、Y’軸重合；A、B、C、D為A’、B’、C’、D’在所述光敏面(5)上對應的成像點；經(jīng)調(diào)整使所述發(fā)射板(3)的Z’軸與所述接收板的Z軸對正時，得到所述接收板與所述發(fā)射板(3)之間的距離0’ F ；步驟(2):對齊接收板與發(fā)射板 信號捕捉所述發(fā)射板(3)沿其X軸、Y軸、Z軸分別移動、轉(zhuǎn)動，直到接收到所述發(fā)射板(3)的所述點光源(4)的成像； 接收板與發(fā)射板的對齊按給定的步長和運動次序移動所述接收板至其與所述發(fā)射板(3)正好對齊，此時所述接收板與所述發(fā)射板(3)姿態(tài)一致，由所述接收板姿態(tài)得知所述發(fā)射板(3)姿態(tài)；對齊判據(jù)為AC 丄 BD,0. 5 (xA + xc) ( 0. lmm、0. 5 (yA + yc) ( 0. 1mm，0. 5(xB + xD) ( 0. lmm、0. 5 (yB + yD) ^ 0. 1mm,其中 0.1mm 為要求的誤差精度。
6.根據(jù)權利要求5所述的測量方法，其特征在于所述發(fā)射板(3)上的五個所述點光源(4)通過單片機控制發(fā)光順序、發(fā)光持續(xù)時間、發(fā)光間隔時間。
全文摘要
本發(fā)明公開了一種三維位姿探測裝置，包括二維位置敏感探測器、凸透鏡、發(fā)射板，所述發(fā)射板上的點光源發(fā)出光線通過所述凸透鏡在所述二維位置敏感探測器的光敏面上成像為清晰點，所述發(fā)射板在中心和四角共設置五個所述點光源，且相鄰兩個角落的所述點光源的距離為20mm；本發(fā)明同時公開了使用這種探測裝置的測量方法。本發(fā)明的優(yōu)點是將二維位置敏感探測器添加光學裝置進行改裝，用于三維空間的位姿檢測，并通過發(fā)射板的布置，結合測量方法協(xié)調(diào)工作，完成空間探測和精準測量；探測裝置結構簡單，控制容易，測量精確度高。
文檔編號G01B11/00GK103063201SQ20121055452
公開日2013年4月24日 申請日期2012年12月19日 優(yōu)先權日2012年12月19日
發(fā)明者張力平, 孫安, 薛培培, 劉克 申請人:江蘇安德信超導加速器科技有限公司, 南京大學, 長安大學, 江蘇德佐電子科技有限公司