專利名稱：一種測量運(yùn)動(dòng)物體六維位姿的設(shè)備的制作方法
技術(shù)領(lǐng)域：
本實(shí)用新型涉及測量運(yùn)動(dòng)物體在較大空間中六維位姿的設(shè)備，具體地 說是一種基于激光束直線特性和并聯(lián)機(jī)構(gòu)位姿測量原理，采用圖像處理技 術(shù)實(shí)現(xiàn)運(yùn)動(dòng)平臺(tái)六維位姿測量的設(shè)備和方法。該可以廣泛應(yīng)用于需要實(shí)現(xiàn) 平移運(yùn)動(dòng)的場合，可以替代昂貴的激光跟蹤儀和坐標(biāo)測量機(jī)。
背景技術(shù)：
對(duì)運(yùn)動(dòng)物體的三維位置(x， y， z)和三維姿態(tài)(ct， (3， y)進(jìn)行高精度 的測量在工業(yè)界有著廣泛的需求。
在很多應(yīng)用中，采用機(jī)械式測量裝置就可以解決問題，例如利用設(shè)備 自身的關(guān)節(jié)編碼器進(jìn)行運(yùn)動(dòng)學(xué)正解，如果設(shè)備本身不具有編碼器或者編碼 器和機(jī)械裝置的精度較低，則可以采用傳統(tǒng)的三坐標(biāo)測量機(jī)和便攜關(guān)節(jié)式 坐標(biāo)測量機(jī)進(jìn)行測量。但是坐標(biāo)測量機(jī)的測量范圍一般十分有限，而且難 以在線測量運(yùn)動(dòng)物體。
為了測量在大范圍運(yùn)動(dòng)的物體的六維位姿，通常采用全局定位系統(tǒng)。 根據(jù)測量精度的高低，全局定位系統(tǒng)可以分為激光跟蹤儀、室內(nèi)GPS、基 于激光導(dǎo)航方法的設(shè)備、基于激光信標(biāo)方法的設(shè)備和基于視覺處理的設(shè)備。
激光跟蹤儀采用經(jīng)緯儀測量接收器的方位，而采用激光干涉原理測量 接收器與經(jīng)緯儀之間的距離，具有采樣頻率高和測量精度高的顯著優(yōu)點(diǎn)， 但是設(shè)備十分昂貴。
室內(nèi)GPS技術(shù)有多種，最典型的一種方案美國Arc Second公司的方案， 在地面安裝至少兩個(gè)激光發(fā)射器，每個(gè)發(fā)射器產(chǎn)生兩個(gè)成固定夾角的激光 平面。發(fā)射器不停高速水平旋轉(zhuǎn)，旋轉(zhuǎn)頻率大約在3000轉(zhuǎn)/分左右，接收器 上的傳感器可獲得激光平面經(jīng)過的準(zhǔn)確時(shí)間，接收器根據(jù)發(fā)射器投射來的 光線時(shí)間特征參數(shù)，計(jì)算出接收器所在點(diǎn)的位置和角度。室內(nèi)GPS的精度 較高，而且可以同時(shí)測量大量接收器的位置，所有接收器在100mxl00m測 量空間內(nèi)的位置測量精度可以達(dá)到O.lmm量級(jí)。不過由于該技術(shù)采用周期 掃描方式，不同激光平面掃過同一傳感器的時(shí)間有時(shí)間差，因此更適合靜 態(tài)物體或者緩慢移動(dòng)物體的測量，如果要用于高速運(yùn)動(dòng)物體的測量，必須 增加激光平面的數(shù)量，或采用超高速旋轉(zhuǎn)的激光發(fā)射器。
基于激光導(dǎo)航方法的設(shè)備和基于激光信標(biāo)方法的設(shè)備主要用于工廠自 動(dòng)移動(dòng)小車的定位，目前主要用于平面位置和方位定位。激光導(dǎo)航方法的典 型例子是在移動(dòng)小車上安裝可以水平旋轉(zhuǎn)的激光站，激光站發(fā)射出激光光
3束，而將三個(gè)以上的反射鏡安裝在地面上，且反射鏡安裝位置己知。地面上 的反射鏡通常由若干個(gè)錐體棱鏡組成，投射到反射鏡上的激光可以沿入射方 向返回，但與入射激光錯(cuò)開小段距離。移動(dòng)小車上激光站不停旋轉(zhuǎn)掃描，如 果在三個(gè)角度有激光返回，則可計(jì)算出移動(dòng)小車的位置和方位。激光信標(biāo)方 法與激光導(dǎo)航方法相反，在小車上安裝三個(gè)以上的傳感器，而在地面安裝多 個(gè)可以水平旋轉(zhuǎn)的激光站，如果三個(gè)以上的傳感器可以接受到激光則可計(jì)算 出移動(dòng)小車的位置和方位。為了實(shí)現(xiàn)連續(xù)測量，激光站的水平轉(zhuǎn)角通常自動(dòng) 跟隨小車的運(yùn)動(dòng)。激光導(dǎo)航方法和激光信標(biāo)方法價(jià)格相對(duì)便宜，其原理可以 擴(kuò)展應(yīng)用到空間六維位姿測量，但是目前尚未見報(bào)道。在這類方法中，傳感 器只是開關(guān)型傳感器，即只檢測信號(hào)的有無，并不測量激光光束在傳感器上 的落點(diǎn)的準(zhǔn)確位置，因此測量精度較低。另一方面，在建筑施工行業(yè)常用的 經(jīng)緯儀交匯法采用可發(fā)射激光的經(jīng)緯儀和能夠判斷激光是否打中靶心的光 電接收靶，可以用于精確測量固定物體的位置和姿態(tài)，但這種測量系統(tǒng)缺乏 跟蹤運(yùn)動(dòng)物體的功能，因此無法測量運(yùn)動(dòng)物體的位姿。
基于視覺處理的全局定位技術(shù)有多種，最典型的是瑞典MEEQ公司的 PosEye技術(shù)，通過在測量環(huán)境中布置多個(gè)位置已知的發(fā)光標(biāo)志物，而在運(yùn) 動(dòng)物體上安置攝像頭，根據(jù)發(fā)光標(biāo)志物在攝像頭拍攝的圖像中的坐標(biāo)來計(jì) 算運(yùn)動(dòng)物體的位姿。當(dāng)測量距離大于10米時(shí)，發(fā)光標(biāo)志物通常采用功率大 于100mW的激光發(fā)光二極管，激光發(fā)光二極管的激光發(fā)散角一般大于30°， 以保證攝像頭能接收到足夠的光線。但是該技術(shù)要求成像單元的分辨率很 高，目前還很難獲得較高的測量精度。另一種常見技術(shù)是雙目視覺技術(shù)， 即在移動(dòng)物體上安裝高亮標(biāo)記物，通過雙目視覺原理來計(jì)算標(biāo)記物的位置， 進(jìn)一步計(jì)算運(yùn)動(dòng)物體的位姿，該技術(shù)同樣受限于成像單元的分辨率，測量 的精度較低。
目前成本較低、不需要高分辨率攝像系統(tǒng)的高精度全局定位設(shè)備還沒 見報(bào)道。

實(shí)用新型內(nèi)容
為了克服現(xiàn)有技術(shù)中成本高、需要高分辨率攝像系統(tǒng)的全局定位設(shè)備 的不足，本實(shí)用新型的目的在于提出一種成本較低、不需要高分辨率攝像 系統(tǒng)的測量運(yùn)動(dòng)物體六維位姿，從而實(shí)現(xiàn)全局定位的設(shè)備，采用本實(shí)用新 型可以替代昂貴的激光跟蹤儀、坐標(biāo)測量機(jī)和室內(nèi)GPS等設(shè)備。
本實(shí)用新型技術(shù)方案是由計(jì)算處理單元、至少一個(gè)接收器和至少一 個(gè)經(jīng)緯儀組成，計(jì)算處理單元與經(jīng)緯儀安裝在固定地面上；接收器安裝在 待測運(yùn)動(dòng)物體上；經(jīng)緯儀和接收器通過通信線纜或者無線通信方式與計(jì)算 處理單元通訊，經(jīng)緯儀與接收器通過激光光路相連；
其中所述經(jīng)緯儀具有至少一個(gè)水平轉(zhuǎn)動(dòng)自由度和至少一個(gè)俯仰轉(zhuǎn)動(dòng) 自由度；經(jīng)緯儀包含驅(qū)動(dòng)裝置，用以控制其水平轉(zhuǎn)角和俯仰轉(zhuǎn)角；還包括轉(zhuǎn)角測量裝置，用以測量轉(zhuǎn)角;每個(gè)經(jīng)緯儀上安裝有至少一個(gè)激光發(fā)射器； 所述經(jīng)緯儀個(gè)數(shù)為1時(shí)，經(jīng)緯儀上安裝的激光發(fā)射器個(gè)數(shù)至少為2;所述每 個(gè)激光發(fā)射器發(fā)射出激光結(jié)構(gòu)光，該激光結(jié)構(gòu)光是至少一條光線的激光點(diǎn) 束，或是至少一個(gè)條形光平面的激光線束，或是至少一個(gè)扇形光平面的激 光線束，或是至少一個(gè)光圓柱面的激光圓形束，或是上述多種不詞激光結(jié) 構(gòu)光的組合光束；
另外，所述經(jīng)緯儀的俯仰自由度可以被固定或者被拆卸，得到具有一 個(gè)水平轉(zhuǎn)動(dòng)自由度的簡化結(jié)構(gòu)，上面安裝有至少一個(gè)激光發(fā)射器，每個(gè)激 光發(fā)射器發(fā)射出 一個(gè)扇形光平面的激光線束；
所述接收器包括至少一塊投射面板和至少一個(gè)成像單元，而且每個(gè)投 射面板至少對(duì)應(yīng)一個(gè)成像單元；投射面板的尺寸以及成像單元與投射面板 的距離均是所述設(shè)備最大測量范圍的百分之一量級(jí) 千分之一量級(jí)范圍內(nèi)； 假設(shè)設(shè)備測量范圍為10m，則投射面板尺寸不大于100mmxl00mm，成像 單元與投射面板的距離在100mm之內(nèi)；其中投射面板是光滑的反光鏡面， 或是表面粗糙的散射反光板，或是散射透光板；投射面板的形狀選自平面， 或是曲面，或是多個(gè)平面的組合，或是多個(gè)曲面的組合；
所述接收器由投射面板和成像單元組成，所述成像單元處于投射面板的 下方、上方或者內(nèi)部；成像單元包括成像電子裝置和位于成像電子裝置與投 射面板之間的成像鏡頭；其中成像電子裝置選自位置敏感探測器、電荷耦合 器件、電荷注入器件、或基于互補(bǔ)金屬氧化物半導(dǎo)體的光學(xué)成像裝置；
所述接收器由投射面板和成像單元組成，所述成像單元直接安裝在投 射面板表面，包括至少一個(gè)感光測量裝置，感光測量裝置選自位置敏感探 測器、電荷耦合器件、電荷注入器件、或基于互補(bǔ)金屬氧化物半導(dǎo)體的光 學(xué)成像裝置。
本實(shí)用新型具有如下特點(diǎn)
1. 本實(shí)用新型設(shè)備成本低而測量精度高。本實(shí)用新型通過兩條以上的 激光結(jié)構(gòu)光實(shí)現(xiàn)采用并聯(lián)測量，與采用單條激光結(jié)構(gòu)光進(jìn)行串聯(lián)測量的激 光跟蹤儀相比，不需要高精度的激光干涉儀，因此成本大大降低；本實(shí)用 新型采用同步測量方式或者輪詢測量方式，與采用周期掃描測量方式的室 內(nèi)GPS系統(tǒng)相比，可以測量運(yùn)動(dòng)速度更高的物體；本實(shí)用新型采用短距離 的視覺捕捉和處理，投射面板與成像單元之間的距離只有最大測量范圍的 百分之一或是千分之一，與采用遠(yuǎn)距離視覺捕捉和處理的PosEye技術(shù)相比， 不需要高分辨率的視覺傳感器，而且定位精度更高。
2. 本實(shí)用新型可以在安裝現(xiàn)場進(jìn)行快速的自行標(biāo)定，因此可以方便的 增加經(jīng)緯儀和接收器，也可以方便的移走冗余的經(jīng)緯儀。
3. 本實(shí)用新型可以通過增加經(jīng)緯儀和接收器的數(shù)量來擴(kuò)大測量范圍和 提高測量結(jié)果的準(zhǔn)確性和魯棒性。例如將三個(gè)經(jīng)緯儀按照三角形分布布置在待測運(yùn)動(dòng)物體周圍，可以使運(yùn)動(dòng)物體在較大空間內(nèi)的位姿測量準(zhǔn)確性波 動(dòng)幅度減小。
4. 以三個(gè)經(jīng)諱儀作為一個(gè)類似蜂窩的測量網(wǎng)格，可以通過坐標(biāo)切換機(jī) 制實(shí)現(xiàn)不同測量網(wǎng)格之間的過渡，實(shí)現(xiàn)大范圍高精度測量。
5. 本實(shí)用新型可以在接收器上安裝測桿或者三維激光掃描儀，從而可
以測量經(jīng)緯儀發(fā)射的激光光束難以達(dá)到的地方。
6. 本實(shí)用新型激光器發(fā)射的激光始終落在接收器上，投入人眼的概率 非常低。即使偶爾進(jìn)入人眼，由于激光可以選擇紅外波段，并且功率非常 低，對(duì)人眼沒有任何傷害。


圖1為六維位姿測量設(shè)備的第一個(gè)實(shí)例的示意圖2為六維位姿測量設(shè)備進(jìn)行實(shí)際測量時(shí)的工作流程示意圖3為六維位姿測量設(shè)備的第二個(gè)實(shí)例的示意圖4為六維位姿測量設(shè)備的第三個(gè)實(shí)例的示意圖5為六維位姿測量設(shè)備的第四個(gè)實(shí)例的示意圖6為六維位姿測量設(shè)備的第五個(gè)實(shí)例的示意圖7為六維位姿測量設(shè)備的第六個(gè)實(shí)例的示意圖8為六維位姿測量設(shè)備的第七個(gè)實(shí)例的示意圖9為六維位姿測量設(shè)備的第八個(gè)實(shí)例的示意圖。
具體實(shí)施方式
以下結(jié)合附圖對(duì)本實(shí)用新型作進(jìn)一步詳述。
如圖1所示為本實(shí)用新型提出的六維位姿測量設(shè)備的第一個(gè)實(shí)例，該 設(shè)備由一個(gè)計(jì)算處理單元l、 一個(gè)接收器2和三個(gè)經(jīng)緯儀3、 4、 5組成。三 個(gè)經(jīng)緯儀3、 4、 5固定安裝在地面上，通常布置成三角形。接收器2通過 連接件7或者直接安裝在六維位姿待測的運(yùn)動(dòng)物體8上。圖中接收器坐標(biāo) 系20為O'-X'Y'Z'，固定在接收器上，而全局坐標(biāo)系10為O-XYZ，固定在 地面上，運(yùn)動(dòng)物體坐標(biāo)系80為Om-XmYmZm，固定在運(yùn)動(dòng)物體8上。由 于接收器坐標(biāo)系20相對(duì)于運(yùn)動(dòng)物體坐標(biāo)系80的位姿是始終固定的，而且 可以通過標(biāo)定方法獲得，因此測量運(yùn)動(dòng)物體8的六維位姿可以通過測量接 收器坐標(biāo)系20相對(duì)于全局坐標(biāo)系10的六維位姿獲得。
經(jīng)緯儀3、 4、 5都具有水平轉(zhuǎn)動(dòng)和俯仰轉(zhuǎn)動(dòng)兩個(gè)轉(zhuǎn)動(dòng)自由度，每個(gè)經(jīng) 緯儀的水平轉(zhuǎn)角和俯仰轉(zhuǎn)角通過驅(qū)動(dòng)裝置進(jìn)行控制，并且轉(zhuǎn)角值可以快速 的測量。驅(qū)動(dòng)裝置通常采用伺服電機(jī)，也可采用精度更高的壓電陶瓷電機(jī)， 轉(zhuǎn)角測量裝置安裝在經(jīng)緯儀上，通常采用高精度編碼器，編碼器分辨率和 測量重復(fù)精度均達(dá)到r量級(jí)。經(jīng)緯儀3、 4、 5上分別安裝有一個(gè)激光發(fā)射 器30、 40、 50。激光發(fā)射器30、 40、 50均發(fā)射至少一條激光束，分別是 31、 41、 51。在實(shí)際應(yīng)用中，為使測量結(jié)果具有更好的準(zhǔn)確性和魯棒性，可以讓每個(gè)激光發(fā)射器發(fā)射兩條或者更多的相互平行的激光光束。激光器安裝在經(jīng)諱儀上，通常采用半導(dǎo)體激光器，波長通常選擇紅色可見光或者紅
外光的波段，如635nm，激光器的輸出功率一般小于5mW。
接收器2主要包括一塊投射面板21和一個(gè)成像單元22。投射面板的尺寸以及成像單元與投射面板的距離均是所述設(shè)備最大測量范圍的百分之一量級(jí) 千分之一量級(jí)；本實(shí)施例設(shè)備測量范圍為10m，則投射面板尺寸不大于100mmxl00mm，成像單元與投射面板的距離在100mm之內(nèi)。本實(shí)例中的投射面板21采用類似360度全景相機(jī)中的雙曲面反光鏡面。成像單元22通常處于投射面板21的下方，通過結(jié)構(gòu)件29與投射面板21相固連。結(jié)構(gòu)件29可以采用透明材料，或者鏤空的不透明材料。成像單元22包括一個(gè)成像鏡頭221和一個(gè)成像電子裝置222。成像電子裝置可以是PSD、 CCD、CMOS等任意光學(xué)成像裝置。當(dāng)成像電子裝置采用PSD時(shí)，由于單個(gè)PSD不能同時(shí)相應(yīng)兩個(gè)或者多個(gè)光點(diǎn)，可以采用多個(gè)PSD組成陣列。
成像鏡頭221的透鏡中心通常放在雙曲面的下側(cè)焦點(diǎn)上，從整個(gè)環(huán)境射向雙曲面上側(cè)焦點(diǎn)的光被反射到下焦點(diǎn)，即成像鏡頭221的透鏡中心。雖然經(jīng)緯儀的轉(zhuǎn)角可以控制，但是很難始終保證經(jīng)諱儀發(fā)出的激光束能射向雙曲面反射鏡的上焦點(diǎn)，因此反光鏡面通常是具有一定的粗糙度的散射反光板，如典型的朗貝反光板(Lambertian reflector)。投射面板21與成像單元22相對(duì)于接收器坐標(biāo)系20的位置和姿態(tài)可以通過標(biāo)定精確獲得。
投射面板21的形狀也可以是平面或其他曲面，當(dāng)投射面板21釆用平面形狀時(shí)，成像單元22推薦采用基于Scheimpflug原理的布局，可以在大范圍內(nèi)獲得清晰的圖像。成像鏡頭221中通常包括帶通濾光鏡片，只允許所采用激光波段的激光能通過，減少環(huán)境光影響，從而提高圖像。
計(jì)算處理單元1通過通信線纜12、 13、 14、 15與接收器12和經(jīng)諱儀3、4、 5相連。
本實(shí)用新型設(shè)備的基本工作過程如下
通過自動(dòng)跟蹤控制步驟控制經(jīng)緯儀的方位角，使經(jīng)緯儀的激光發(fā)射器始終指向接收器的投射面板，激光發(fā)射器發(fā)射激光線條或者激光平面投射在投射面板上，產(chǎn)生激光光斑或者激光條紋。如圖1所示，經(jīng)緯儀3、 4、 5上的激光發(fā)射器30、 40、 50發(fā)射的激光束31、 41、 51落在接收器上的投射面板上，形成相應(yīng)的激光光斑32、 42、 52。根據(jù)目前的小型半導(dǎo)體激光器技術(shù)水平，在10m距離處，激光光斑的直徑通常小于0.5mm，在100m距離處，激光光斑的直徑通常小于5mm。
再通過對(duì)經(jīng)緯儀位姿的標(biāo)定獲得經(jīng)緯儀相對(duì)于固定在地面上的全局坐標(biāo)系的位姿，進(jìn)一步通過經(jīng)緯儀的旋轉(zhuǎn)變換矩陣獲得激光發(fā)射器發(fā)射的激光線條或者激光平面在全局坐標(biāo)系中的數(shù)學(xué)方程。 一方面，經(jīng)緯儀3、 4、 5相對(duì)于絕對(duì)坐標(biāo)系IO的位姿可以采用外部設(shè)備預(yù)先測定，也可以采用本專利后面介紹的自行標(biāo)定方法獲得。再結(jié)合經(jīng)緯儀水平轉(zhuǎn)角和俯仰轉(zhuǎn)角的測量值就可以獲得三條激光點(diǎn)束31、 41、 51在全局坐標(biāo)系10中的方程，每
條激光束對(duì)應(yīng)兩個(gè)方程。假設(shè)三條激光點(diǎn)束31、 41、 51對(duì)應(yīng)的代數(shù)方程分
上述方程的系數(shù)只取決于經(jīng)緯儀3、 4、 5在全局坐標(biāo)系10中的位姿和經(jīng)緯儀3、 4、 5的轉(zhuǎn)角測量值。
另一方面，通過成像單元對(duì)投射面板上的激光光斑或者激光條紋進(jìn)行拍攝和圖像處理，獲得激光光斑或者激光條紋在成像單元中的映像的像素坐標(biāo)；通過對(duì)成像單元的標(biāo)定獲得成像單元上像素坐標(biāo)中每一個(gè)像素點(diǎn)相對(duì)于接收器坐標(biāo)系的位置，利用像素點(diǎn)與接收器坐標(biāo)系的一一對(duì)應(yīng)關(guān)系獲得激光光斑或者激光條紋相對(duì)于接收器坐標(biāo)系的局部坐標(biāo)值。如圖1所示，通過對(duì)成像電子裝置222拍攝的圖像進(jìn)行處理，可以獲得激光光斑在成像電子裝置222上的坐標(biāo)。根據(jù)小孔成像原理，激光光斑與其在成像電子裝置222上的映像之間的連線必然經(jīng)過成像鏡頭221的透鏡中心。由于投射面板21、成像鏡頭221和成像電子裝置222相對(duì)于接收器坐標(biāo)系20的位置是固定的，因此可以通過小孔成像數(shù)學(xué)模型獲得這些激光光斑相對(duì)于接收器坐標(biāo)系20的三維位置；或者采用直接標(biāo)定方法，即對(duì)于成像電子裝置222上的每一個(gè)像素，直接標(biāo)定并記錄下其在投射面板21上對(duì)應(yīng)點(diǎn)在接收器坐標(biāo)系20中的坐標(biāo)。假設(shè)投射面板21的大小為100mmxl00mm，成像電子裝置222的像素陣列為1024像素xl024像素，成像單元22的視場大小與投射面板大小基本相等，則成像電子裝置222的視覺分辨率小于O.lmm。
再通過假設(shè)接收器坐標(biāo)系相對(duì)于全局坐標(biāo)系的六維位姿獲得成像單元標(biāo)定得到的激光光斑或者激光條紋相對(duì)于全局坐標(biāo)系中的全局坐標(biāo)值。由于接收器坐標(biāo)系20相對(duì)于全局坐標(biāo)系10的位姿是六個(gè)未知變量(x， y， z，a, (3， y)，如果已知一個(gè)激光光斑在接收器坐標(biāo)系20中的相對(duì)坐標(biāo)(X'，Y'， Z')，可以根據(jù)以下齊次坐標(biāo)變換獲得該激光光斑在全局坐標(biāo)系IO中的三維全局坐標(biāo)(X， Y， Z):
a41x + 641少+ c41z + <i41 = 0二 rra似O, z)i o/(Z, "i o《X， j3)7 o/(Z， a)
1
z'1
其中7 o"Z,"i W(X,戶)7 o/(Z,cO表示先繞全局坐標(biāo)系的Z軸旋轉(zhuǎn)a角度，接著繞X軸旋轉(zhuǎn)/ 角度，再繞Z軸旋轉(zhuǎn)y角度，7Va似(x,y,z)表示沿著矢量[x,3^]平移。因此可以獲得
r12n3Z'
&少7'
zr33Z'
100011
其中，
^, = cos y cos a - cos j0 sin a sin yr12 = cos y si緒+ cos j8 cos a sin yr13 = sin y sin j8
r21 = - sin y cos a — cos /3 sin a cos yr22 = _ sin y sin a + cos ^ cos a cos yr23 = cos y sinr31 = sin sincer32 = - sin cos ar33 = cos j3
可見給定一個(gè)激光光斑在接收器坐標(biāo)系20中的相對(duì)坐標(biāo)(X'， Y'， Z')，該激光光斑在全局坐標(biāo)系IO中的全局坐標(biāo)(X， Y， Z)分別對(duì)應(yīng)三個(gè)以接收器坐標(biāo)系20的六維位姿(x， y， z， ot， p， Y)為變量的函數(shù)表達(dá)式。
最后通過同步觸發(fā)測量步驟建立激光結(jié)構(gòu)光生成的直線或者平面在全局坐標(biāo)系中的數(shù)學(xué)方程和成像單元標(biāo)定得到的激光光斑或者激光條紋相對(duì)于全局坐標(biāo)系的全局坐標(biāo)值之間的約束關(guān)系，求解約束關(guān)系得出接收器坐標(biāo)系相對(duì)于全局坐標(biāo)系的位姿。由于三個(gè)光斑32、 42、 52必須分別位于三條激光束對(duì)應(yīng)的直線上，將三個(gè)光斑的三維全局坐標(biāo)對(duì)應(yīng)的函數(shù)表達(dá)式分別代入相應(yīng)激光束的兩個(gè)方程，可以獲得具有六個(gè)變量的六個(gè)方程，
9["3,32 +631^32 +C31Z32 +"31 = W(X乂Z，",/^，？0 = 0
le31x32+/31>y32+g31z32+/z31 ^(x,少，z，a，〃，"二0 j "41x42 + 641_y42 + c41z42 + J41 = r(x, 乂 z, a, j8，" = 0 le41x42 + /41y42 + g41z42 + & = 少," = 0
"51X52 + + C51Z52 +《1 =乂 Z， a, /3, " 二 0
e51x52 + /51y52 + g51z52 + /251 = Q(x， 乂 ； a ， /3，" = 0
在以上六個(gè)方程中，假設(shè)激光光斑32的全局坐標(biāo)為(x32， y32， z32)， 激光光斑42的全局坐標(biāo)為(x42， y42， z42)，激光光斑52的全局坐標(biāo)為(x52， y52， z52)，顯然它們都是以接收器坐標(biāo)系20的六維位姿(x， y， z， a，卩， y)為變量的函數(shù)表達(dá)式。
求解以上六個(gè)方程所組成的方程組可以獲得接收器坐標(biāo)系20相對(duì)于全 局坐標(biāo)系10的六維位姿(x， y， z， a， (3， Y)。求解方程組的算法可以是 經(jīng)典的牛頓-拉夫遜方法、各種最優(yōu)方法、同倫法和區(qū)間分析法等等。
事實(shí)上，上述激光光斑必須分別位于激光束對(duì)應(yīng)的直線上的約束關(guān)系 可以采用其他表達(dá)方法，例如三個(gè)光斑32、 42、 52與三條激光束的距離等 于零，并獲得不同的數(shù)學(xué)方程。接收器坐標(biāo)系20相對(duì)于全局坐標(biāo)系10的 六維位姿也可以采用其他等效表示方法進(jìn)行表示，例如三維位置采用(x， y， z)表示，而三維姿態(tài)(ct， (3， y)采用四元數(shù)表示，或者三維位置和三 維姿態(tài)統(tǒng)一采用雙四元數(shù)表示。
圖2為六維位姿測量設(shè)備進(jìn)行實(shí)際測量時(shí)的工作流程示意圖，包括經(jīng) 緯儀位姿自行標(biāo)定的步驟和同步觸發(fā)測量的步驟。該工作流程針對(duì)第一個(gè) 實(shí)例進(jìn)行解釋，但是可以推廣用于后述的其他實(shí)例。假設(shè)接收器2可放置 在一個(gè)初始位置，在該初始位置時(shí)，接收器坐標(biāo)系20與全局坐標(biāo)系10重 合。首先需要標(biāo)定三個(gè)經(jīng)緯儀3、 4、 5的六維位姿，共有十八個(gè)未知變量， 因此需構(gòu)建至少十八個(gè)方程才能得到一個(gè)確定性方程組，從而求解出這十 八個(gè)變量。具體工作流程如下
首先執(zhí)行經(jīng)緯儀位姿自行標(biāo)定方法的步驟。在步驟IOI，將三個(gè)經(jīng)緯儀 3、 4、 5固定在地面。步驟102將接收器2移動(dòng)到初始位置，使接收器坐標(biāo) 系20與全局坐標(biāo)系IO重合。在步驟103，調(diào)節(jié)經(jīng)緯儀3、 4、 5的水平轉(zhuǎn)角 和俯仰轉(zhuǎn)角使激光光束落在接收器2的投射面板21上。在步驟104，計(jì)算 處理單元1發(fā)送觸發(fā)信號(hào)分別給經(jīng)緯儀3、 4、 5和接收器2。在步驟105， 經(jīng)諱儀3、4、5將觸發(fā)時(shí)刻的水平轉(zhuǎn)角和俯仰角數(shù)據(jù)發(fā)送給計(jì)算處理單元1， 與此同時(shí)接收器2的成像單元22觸發(fā)高速快門，捕獲投射面板21上的光 斑圖像，并將計(jì)算獲得的激光光斑相對(duì)于接收器坐標(biāo)系10的局部坐標(biāo)值發(fā)
10送給計(jì)算處理單元1。在步驟106計(jì)算處理單元1根據(jù)激光光斑位于激光直 線上的約束關(guān)系，將激光光斑在全局坐標(biāo)系中的函數(shù)表達(dá)式代入激光線條 或者結(jié)構(gòu)光平面的數(shù)學(xué)方程，建立以三個(gè)經(jīng)緯儀的18個(gè)位姿參數(shù)為未知變
量的第l-6個(gè)方程。第二次執(zhí)行步驟103，調(diào)節(jié)經(jīng)緯儀3、 4、 5的水平轉(zhuǎn)角 和俯仰轉(zhuǎn)角到第二組數(shù)值，但仍使激光光束落在接收器的投射面板上。然 后依次執(zhí)行步驟104、 105和106，獲得第7-12個(gè)方程。第三次執(zhí)行重復(fù)步 驟103，調(diào)節(jié)經(jīng)諱儀3、 4、 5的水平轉(zhuǎn)角和俯仰轉(zhuǎn)角到第三組數(shù)值，但仍使 激光光束落在接收器的投射面板上。然后依次執(zhí)行步驟104、 105和106， 獲得第13-18個(gè)方程。在步驟107，求解具有十八個(gè)變量和十八個(gè)方程的方 程組，得出三個(gè)經(jīng)緯儀相對(duì)于全局坐標(biāo)系10的位姿參數(shù)，完成經(jīng)緯儀位姿 自行標(biāo)定。
然后執(zhí)行同步觸發(fā)測量的步驟。在步驟108，將接收器2固定安裝在待 測移動(dòng)物體上。在步驟109，啟動(dòng)自動(dòng)跟蹤控制算法，控制經(jīng)緯儀3、 4、 5 的水平轉(zhuǎn)角和俯仰轉(zhuǎn)角來跟蹤接收器2的運(yùn)動(dòng)，使激光光束落在接收器2 的投射面板21上。然后重復(fù)歩驟104、步驟105和步驟106，在步驟110， 求解在步驟106獲得的六個(gè)方程，獲得接收器坐標(biāo)系20在全局坐標(biāo)系10 中的六維位姿。此后循環(huán)執(zhí)行步驟109、步驟104、步驟105、步驟106和 步驟110，即可連續(xù)獲得接收器坐標(biāo)系20在全局坐標(biāo)系10中的六維位姿。
上述使經(jīng)緯儀自動(dòng)跟蹤接收器2運(yùn)動(dòng)的自動(dòng)跟蹤控制算法的流程簡介 如下，在每次測量時(shí)，由計(jì)算處理單元1計(jì)算出接收器2的當(dāng)前位姿，將 當(dāng)前位姿與接收器2在上一次測量時(shí)的位姿之差除以采樣間隔得出接收器2 的運(yùn)動(dòng)速度，進(jìn)一步根據(jù)接收器2的運(yùn)動(dòng)速度推測接收器2在下一次測量 時(shí)將到達(dá)的位姿，計(jì)算出所有經(jīng)緯儀3、 4、 5需要到達(dá)的新轉(zhuǎn)角位置，以 保證激光光斑32、 42、 52仍然落在接收器2的投射面板21上，并將轉(zhuǎn)角 指令發(fā)送給相應(yīng)的經(jīng)緯儀32、 42、 52。求解接收器2的運(yùn)動(dòng)速度還可以利 用接收器2之前的運(yùn)動(dòng)軌跡，通過濾波和預(yù)測方法獲得。
上述的同步觸發(fā)測量方法也可以采用連續(xù)測量和測量值插補(bǔ)相結(jié)合的 方法代替，即計(jì)算處理單元1不必發(fā)送觸發(fā)信號(hào)給經(jīng)緯儀3、 4、 5和接收器 2，而經(jīng)緯儀3、 4、 5連續(xù)將水平轉(zhuǎn)角和俯仰角的最新數(shù)據(jù)發(fā)送給計(jì)算處理 單元1，接收器2也連續(xù)將計(jì)算獲得的激光光斑相對(duì)于接收器坐標(biāo)系10的最 新局部坐標(biāo)值發(fā)送給計(jì)算處理單元1。計(jì)算處理單元1根據(jù)接收到的數(shù)值和 接收到數(shù)值的時(shí)間插補(bǔ)出對(duì)應(yīng)某固定時(shí)刻的數(shù)值，進(jìn)一步根據(jù)插補(bǔ)出的數(shù)值 建立方程組并求解出接收器坐標(biāo)系20在全局坐標(biāo)系10中的六維位姿。
本設(shè)備的位置測量精度和姿態(tài)測量精度可以達(dá)到如下指標(biāo)采用高精 度角編碼器時(shí)，普通經(jīng)緯儀的轉(zhuǎn)角動(dòng)態(tài)測量精度可以達(dá)到2"以下，轉(zhuǎn)角靜 態(tài)測量精度可以達(dá)到r以下。假設(shè)所采用的經(jīng)緯儀的轉(zhuǎn)角精度為2"，經(jīng)緯 儀與測量物體的距離為10m，經(jīng)緯儀發(fā)射的激光束在10m距離處的置信區(qū)間半徑小于O.lmm。而采用成像單元的圖像分辨率可以輕易的達(dá)到0.1mm 以下，例如采用1024像素xl024像素的成像電子裝置拍攝面積為 50mmx50mm的投射面板，可以獲得0.05mm的分辨率。采用本實(shí)用新型方 案，觸發(fā)信號(hào)的傳輸延時(shí)和成像單元的快門曝光時(shí)間都可以達(dá)到lO)is量級(jí)， 當(dāng)運(yùn)動(dòng)物體速度為lm/s時(shí)，成像單元的測量偏差為O.Olmm量級(jí)。由于本 實(shí)用新型采用多組方程進(jìn)行求解，具有類似并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)學(xué)求逆解的特點(diǎn)， 多個(gè)激光束的指向誤差不累積，因此本實(shí)用新型設(shè)備的位置測量精度可以 達(dá)到O.lmm量級(jí)。已知接收器的投射面板的尺寸為50mmx50mm，假設(shè)激 光光斑不規(guī)則的落在投射面板上，其中所有激光光斑與全部激光光斑的幾 何重心之間的最小距離為5mm，則在10m距離處，姿態(tài)測量精度可以達(dá)到 (0.1/5) x (1S07兀)=1.14°，即1°量級(jí)。在后述的第八個(gè)實(shí)例中，在運(yùn)動(dòng) 物體上安裝三個(gè)接收器并使接收器之間的距離達(dá)到50mm，本實(shí)用新型設(shè)備 的姿態(tài)測量精度可以達(dá)到0. 1°量級(jí)。本實(shí)例的投射面板21只能接收來自四 周的激光束，而不能接收來自上方和下方的激光束，因此可以測量的運(yùn)動(dòng) 物體姿態(tài)變化范圍較小， 一般在100°左右。在后述實(shí)例中將介紹一些姿態(tài) 測量范圍更大的接收器。
本設(shè)備的自動(dòng)跟蹤控制步驟的跟蹤性能可以達(dá)到如下指標(biāo)假設(shè)自動(dòng) 跟蹤控制算法的控制周期為100Hz，運(yùn)動(dòng)物體與經(jīng)緯儀的距離為10m，當(dāng) 運(yùn)動(dòng)物體運(yùn)動(dòng)速度為lm/s時(shí)，在每個(gè)控制周期，運(yùn)動(dòng)物體移動(dòng)距離的預(yù)測 準(zhǔn)確度為10士2mm。假設(shè)經(jīng)緯儀的水平轉(zhuǎn)角和俯仰轉(zhuǎn)角采用位置閉環(huán)控制， 采用伺服電機(jī)和機(jī)械傳動(dòng)方式時(shí)，轉(zhuǎn)角的控制準(zhǔn)確度為±0.05°，因此激光光 斑在投射面板上落點(diǎn)的準(zhǔn)確度為9mm。假設(shè)接收器的投射面板的尺寸為 50mmx50mm，每個(gè)控制周期都可以控制激光光斑落在以投射面板中心為圓 心而半徑為19mm的圓以內(nèi)，即激光光斑始終落在投射面板上，因此本設(shè) 備可以跟蹤距離在10米外、運(yùn)動(dòng)速度在lm/s的運(yùn)動(dòng)物體。當(dāng)測量距離為 100m的運(yùn)動(dòng)物體時(shí)，可以采用壓電陶瓷電機(jī)或者直接驅(qū)動(dòng)電機(jī)來提高經(jīng)緯 儀轉(zhuǎn)角控制精度，轉(zhuǎn)角控制準(zhǔn)確度可以達(dá)到±0.005°以下，同時(shí)還可以通過 縮短跟蹤控制的控制周期和增大投射面板尺寸等措施來保證準(zhǔn)確的跟蹤。
圖3所示為本實(shí)用新型提出的六維位姿測量設(shè)備的第二個(gè)實(shí)例，該設(shè) 備由一個(gè)計(jì)算處理單元l、 一個(gè)接收器2和三個(gè)經(jīng)緯儀3、 4、 5組成。與第 一個(gè)實(shí)例不同的是，該設(shè)備中三個(gè)經(jīng)緯儀3、 4、 5上的激光發(fā)射器30、 40、 50發(fā)射的是生成條形光平面的激光線束33、 43、 53。該設(shè)備中的接收器2 采用了六面體形狀，接收器2中的成像單元也采用另一種方式，即不采用 基于透鏡成像的成像電子裝置，而是在投射面板21上直接安裝感光測量裝 置23、 24、 25等，感光測量裝置可以包括一個(gè)或者多個(gè)PSD、 CCD、 CMOS 等感光傳感器。感光測量裝置表面通常覆蓋帶通濾光鏡片，只允許所采用 激光波段的激光能通過，減少環(huán)境光影響，從而提高圖像。濾光鏡片的厚度要求較薄，避免由于折射影響測量精度。
本實(shí)例的工作原理與第一個(gè)實(shí)例基本相同激光線束33、 43、 53生成
三個(gè)光平面，根據(jù)經(jīng)緯儀水平轉(zhuǎn)角和俯仰轉(zhuǎn)角的測量值就可以獲得三個(gè)激
光線束33、 43、 53對(duì)應(yīng)的光平面在全局坐標(biāo)系10中的方程，每個(gè)激光線 束生成的光平面對(duì)應(yīng)一個(gè)方程，假設(shè)光平面33、 43、 53對(duì)應(yīng)的代數(shù)方程分 別為
a33x + 633_y + c33z + <i33 =0 a43x + 643_y + c43z + <i43 = 0 a53x + 653少+ c53z + d53 =0
上述方程的系數(shù)只取決于經(jīng)緯儀3、 4、 5在全局坐標(biāo)系10中的位姿和 經(jīng)緯儀3、 4、 5的轉(zhuǎn)角測量值。
激光線束33、 43、 53落在感光測量裝置23、 24、 25上形成至少三條 激光條紋26、 27、 28。感光測量裝置23、 24、 25等可以獲得激光條紋26、 27、 28相對(duì)于感光測量裝置基準(zhǔn)的像素坐標(biāo)，由于感光測量裝置23、 24、 25相對(duì)于接收器坐標(biāo)系20的位置是固定不變而且可以通過標(biāo)定方法獲得， 因此可以獲得這些激光條紋相對(duì)于接收器坐標(biāo)系20的三維位置。激光條紋 上的每一個(gè)點(diǎn)在全局坐標(biāo)系10中的全局坐標(biāo)都是以接收器坐標(biāo)系20六維 位姿為變量的函數(shù)表示式。在激光條紋26、 27、 28上分別選取兩個(gè)點(diǎn)，總 共六個(gè)點(diǎn)。這六個(gè)點(diǎn)分別位于光平面33、 43、 53上，因此可以獲得具有六 個(gè)變量的六個(gè)方程。求解該方程組成的方程組可以獲得接收器坐標(biāo)系20相 對(duì)于全局坐標(biāo)系IO的六維位姿。采用激光線束的好處是可以在每條激光條 紋上選取兩個(gè)以上的點(diǎn)，從而獲得更多的約束方程，提高方程組求解的準(zhǔn) 確性和可靠性。
圖4所示為本實(shí)用新型提出的六維位姿測量設(shè)備的第三個(gè)實(shí)例，該設(shè) 備由一個(gè)計(jì)算處理單元l、 一個(gè)接收器2和兩個(gè)經(jīng)緯儀3、 4組成。其中經(jīng) 緯儀3上的激光發(fā)射器發(fā)射30發(fā)射出生成光平面的激光線束33，而經(jīng)緯儀 4上的激光發(fā)射器發(fā)射40發(fā)射至少兩條激光束41、 42。根據(jù)前面兩個(gè)實(shí)例
的分析方法不難得到具有六個(gè)變量的六個(gè)方程，因此可以獲得接收器坐標(biāo) 系20相對(duì)于全局坐標(biāo)系10的六維位姿。
圖5所示為本實(shí)用新型提出的六維位姿測量設(shè)備的第四個(gè)實(shí)例，該實(shí) 例具有部件數(shù)量較少的特點(diǎn)。該設(shè)備由一個(gè)計(jì)算處理單元l、 一個(gè)接收器2 和一個(gè)經(jīng)緯儀6組成。經(jīng)緯儀6與前述經(jīng)緯儀3、 4、 5的不同之處是增加 了一個(gè)俯仰轉(zhuǎn)動(dòng)自由度，在兩個(gè)俯仰轉(zhuǎn)動(dòng)自由度上分別安裝有激光發(fā)射器 60、 61。激光發(fā)射器60發(fā)射出兩條激光束62、 63，而激光發(fā)射器61發(fā)射 出兩條激光束64、 65。激光束62、 63、 64、 65落在接收器2的投射板21 上形成四個(gè)光斑。根據(jù)第一個(gè)實(shí)例的分析方法不難得到具有六個(gè)變量的八個(gè)方程，因此可以獲得接收器坐標(biāo)系20相對(duì)于全局坐標(biāo)系IO的六維位姿。 在此指出，雖然三條激光束即可得到具有六個(gè)變量的六個(gè)方程，但是采用 四條激光束有利于提高系統(tǒng)測量結(jié)果的準(zhǔn)確性和魯棒性。接收器2與計(jì)算
處理單元1之間的通信采用無線通信12'取代通信線纜12，從而避免運(yùn)動(dòng)物 體8與通信線纜12的運(yùn)動(dòng)干涉。
圖6所示為本實(shí)用新型提出的六維位姿測量設(shè)備的第五個(gè)實(shí)例，該實(shí)例 與第四個(gè)實(shí)例的區(qū)別在于，接收器2上的投射面板21為球面狀曲面，采用較 薄的散射透光材料。成像單元22從投射面板21內(nèi)部拍攝投射面板21上出現(xiàn) 的激光光斑。成像單元22的成像鏡頭221采用廣角近攝鏡頭，例如最短工作 距離在10 200mm左右的廣角近攝鏡頭。根據(jù)第一個(gè)實(shí)例的分析方法不難得 到具有六個(gè)變量的八個(gè)方程，因此可以獲得接收器坐標(biāo)系20相對(duì)于全局坐標(biāo) 系10的六維位姿。與第一個(gè)實(shí)例相比，本實(shí)例的投射面板可以接收來自更大 范圍的激光，可以測量的運(yùn)動(dòng)物體姿態(tài)變化范圍可以達(dá)到270°以上。
圖7所示為本實(shí)用新型提出的六維位姿測量設(shè)備的第六個(gè)實(shí)例，該實(shí) 例與第四個(gè)實(shí)例的區(qū)別在于，接收器2上具有兩塊投射面板21和21'，以 及兩個(gè)成像單元22和22'。投射面板21和投射面板21'都是曲面薄殼，其中 投射面板21采用較薄的散射透光材料，而投射面板21'采用相對(duì)較厚的散 射透光材料，在兩個(gè)投射面板上均有一部分激光被反射，而另一部分激光 通過。經(jīng)緯儀6上的激光發(fā)射器60、 61發(fā)射出兩個(gè)激光點(diǎn)束62、 64。激光 點(diǎn)束62在投射面板21和21'上分別形成兩個(gè)激光光斑66、 67，激光點(diǎn)束 64在投射面板21和21'上分別形成兩個(gè)激光光斑68、 69。成像單元22拍 攝投射面板21上的激光光斑，而成像單元22'拍攝投射面板21'上的激光光 斑。根據(jù)第一個(gè)實(shí)例的分析方法不難得到具有六個(gè)變量的八個(gè)方程，因此 可以獲得接收器坐標(biāo)系20相對(duì)于全局坐標(biāo)系10的六維位姿。事實(shí)上，由 于投射面板21是較薄的散射透光材料，投射面板21'上的光斑也可能透過 投射面板21在成像單元22中成像，因此成像單元22'可以省略，在這種情 況下，要求成像單元22的圖像處理算法能排除由于多重反射形成的干擾光 斑。
圖8所示為本實(shí)用新型提出的六維位姿測量設(shè)備的第七個(gè)實(shí)例，該設(shè) 備由一個(gè)計(jì)算處理單元1、 一個(gè)接收器2和兩個(gè)結(jié)構(gòu)經(jīng)過簡化的經(jīng)緯儀9、 11組成。該設(shè)備中的經(jīng)緯儀9、 11的俯仰自由度被固定或者被移除，接收 器2采用了六面體形狀。經(jīng)緯儀9、 11只具有一個(gè)水平旋轉(zhuǎn)自由度。經(jīng)諱 儀9、 11上安裝激光發(fā)射器91、 111。激光發(fā)射器91發(fā)射出一個(gè)扇形光平 面的激光線束92，激光發(fā)射器111發(fā)射出一個(gè)扇形光平面的激光線束112， 而且激光線束92和激光線束112通常與地面垂直。激光線束92投射到接 收器2上的投射面板21上生成兩段激光條紋93、 94，激光線束92投射到 投射面板21上生成兩段激光條紋113、 114。根據(jù)第二個(gè)實(shí)例的分析方法不
14難得到具有六個(gè)變量的八個(gè)方程，因此可以獲得接收器坐標(biāo)系20相對(duì)于全 局坐標(biāo)系10的六維位姿。
圖9所示為本實(shí)用新型提出的六維位姿測量設(shè)備的第八個(gè)實(shí)例。三個(gè)
經(jīng)緯儀3、 4、 5固定在地面上，通常布置成三角形。移動(dòng)物體8上安裝三 個(gè)分開放置的接收器20、 20a、 20b，接收器20與計(jì)算處理單元1通過無線 通信12'進(jìn)行連接，接收器20a、 20b與接收器20之間通過有線通信或者無 線通信方式進(jìn)行連接。其中接收器2對(duì)應(yīng)的接收器坐標(biāo)系20為O'-X'Y'Z'， 接收器2a對(duì)應(yīng)的接收器坐標(biāo)系20a為O"-X"Y "Z"，接收器2b對(duì)應(yīng)的接收 器坐標(biāo)系20b為O"'-X"'Y"'Z"'。經(jīng)緯儀3、 4、 5上分別安裝有一個(gè)激光發(fā)射 器30、 40、 50。激光發(fā)射器30、 40、 50均發(fā)射至少一條激光束，分別是 31、 41、 51。激光束31、 41、 51分別落在接收器2、 2a、 2b的投射面板21、 21a、 21b上，生成三個(gè)激光光斑32、 42、 52。在實(shí)際測量時(shí)，每個(gè)經(jīng)緯儀 優(yōu)先將激光結(jié)構(gòu)光投射到距離該經(jīng)緯儀較近而且未被遮擋的接收器上。由 于本實(shí)例中的接收器20、 20a、 20b之間的距離通常遠(yuǎn)大于接收器的尺寸， 因此獲得更準(zhǔn)確的姿態(tài)測量結(jié)果。
本實(shí)例的工作原理如下在實(shí)際測量之前先標(biāo)定各個(gè)接收器之間的相 對(duì)位姿。選取接收器2為主接收器，使運(yùn)動(dòng)物體8固定不動(dòng)，并測量所有 接收器坐標(biāo)系20、 20a、 20b相對(duì)于全局坐標(biāo)系10的六維位姿，通過坐標(biāo) 變換矩陣可以獲得其他兩個(gè)接收器2a、 2b上的接收器坐標(biāo)系20a、 20b相對(duì) 于主接收器2的接收器坐標(biāo)系20的相對(duì)六維位姿。在實(shí)際測量時(shí)，運(yùn)動(dòng)物 體8開始運(yùn)動(dòng)，接收器2a將拍攝和處理獲得的激光光斑42在本接收器坐 標(biāo)系20a上的坐標(biāo)值發(fā)送給接收器20，接收器2b將拍攝和處理獲得的激光 光斑52在本接收器坐標(biāo)系20b上的坐標(biāo)值發(fā)送給接收器20，接收器20根 據(jù)接收器坐標(biāo)系20a、 20b相對(duì)于接收器坐標(biāo)系20的位姿關(guān)系，獲得接收 器2a、 2b上的激光光斑42、 52在接收器坐標(biāo)系20中的局部坐標(biāo)，然后采 用第一個(gè)實(shí)例中論述的方程組構(gòu)建方法計(jì)算得到接收器坐標(biāo)20相對(duì)于全局 坐標(biāo)系10的六維位姿。
需要指出的是，雖然這里只給出了六維位姿測量設(shè)備的八個(gè)實(shí)例，但 是通過對(duì)上述實(shí)例的部件進(jìn)行組合可以得到更多的實(shí)例。此外，在上述八 個(gè)實(shí)例中，經(jīng)緯儀安裝在固定地面上，接收器安裝在待測運(yùn)動(dòng)物體上，但 是也可以將經(jīng)緯儀和接收器中的一種安裝在待測運(yùn)動(dòng)物體上，另一種安裝 在固定地面上。
1權(quán)利要求1. 一種測量運(yùn)動(dòng)物體六維位姿的設(shè)備，其特征在于由計(jì)算處理單元(1)、至少一個(gè)接收器和至少一個(gè)經(jīng)緯儀組成；計(jì)算處理單元與經(jīng)緯儀安裝在固定地面上；接收器安裝在待測運(yùn)動(dòng)物體(8)上；經(jīng)緯儀和接收器與計(jì)算處理單元(1)通訊，經(jīng)緯儀與接收器通過激光光路相連。
2. 按權(quán)利要求1所述的設(shè)備，其特征在于所述經(jīng)緯儀具有至少一個(gè) 水平轉(zhuǎn)動(dòng)自由度和至少一個(gè)俯仰轉(zhuǎn)動(dòng)自由度；經(jīng)緯儀包含驅(qū)動(dòng)裝置，用以 控制其水平轉(zhuǎn)角和俯仰轉(zhuǎn)角；還包含轉(zhuǎn)角測量裝置，用以測量其水平轉(zhuǎn)角和 俯仰轉(zhuǎn)角；經(jīng)緯儀上安裝有至少一個(gè)激光發(fā)射器。
3. 按權(quán)利要求l所述的設(shè)備，其特征在于所述經(jīng)緯儀具有一個(gè)水平 轉(zhuǎn)動(dòng)自由度，還配有控制所述水平轉(zhuǎn)動(dòng)自由度的驅(qū)動(dòng)裝置及用以測量水平 轉(zhuǎn)角的轉(zhuǎn)角測量裝置；經(jīng)緯儀上安裝有至少一個(gè)激光發(fā)射器。
4. 按權(quán)利要求2或3所述的設(shè)備，其特征在于所述經(jīng)緯儀個(gè)數(shù)為l 時(shí)，經(jīng)緯儀上安裝的激光發(fā)射器個(gè)數(shù)至少為2。
5. 按權(quán)利要求2所述的設(shè)備，其特征在于所述每個(gè)激光發(fā)射器發(fā)射 出激光結(jié)構(gòu)光，該激光結(jié)構(gòu)光是至少一條光線的激光點(diǎn)束，或是至少一個(gè) 條形光平面的激光線束，或是至少一個(gè)扇形光平面的激光線束，或是至少 一個(gè)光圓柱面的激光圓形束，或是上述多種不同激光結(jié)構(gòu)光的組合光束。
6. 按權(quán)利要求3所述的設(shè)備，其特征在于所述每個(gè)激光發(fā)射器發(fā)射 出 一個(gè)扇形光平面的激光線束。
7. 按權(quán)利要求l所述的設(shè)備，其特征在于所述接收器包括至少一塊 投射面板和至少一個(gè)成像單元，而且每個(gè)投射面板至少對(duì)應(yīng)一個(gè)成像單元; 其中投射面板是光滑的反光鏡面，或是表面粗糙的散射反光板，或是散射 透光板；投射面板的形狀選自平面，或是曲面，或是多個(gè)平面的組合，或 是多個(gè)曲面的組合。
8. 按權(quán)利要求7所述的設(shè)備，其特征在于所述接收器由投射面板和 成像單元組成，所述成像單元處于投射面板的下方、上方或者內(nèi)部，包括 成像電子裝置和位于成像電子裝置與投射面板之間的成像鏡頭；其中成像 電子裝置選自位置敏感探測器、電荷耦合器件、電荷注入器件、或基于互 補(bǔ)金屬氧化物半導(dǎo)體的光學(xué)成像裝置。
9. 按權(quán)利要求7所述的設(shè)備，其特征在于所述接收器由投射面板和 成像單元組成，所述成像單元直接安裝在投射面板表面，包括至少一個(gè)感 光測量裝置，感光測量裝置選自位置敏感探測器、電荷耦合器件、電荷注 入器件、或基于互補(bǔ)金屬氧化物半導(dǎo)體的光學(xué)成像裝置。
專利摘要本實(shí)用新型涉及一種測量運(yùn)動(dòng)物體六維位姿的設(shè)備。該設(shè)備由一個(gè)或多個(gè)經(jīng)緯儀、一個(gè)或多個(gè)接收器和一個(gè)計(jì)算處理單元組成，每個(gè)經(jīng)緯儀的水平轉(zhuǎn)角和俯仰轉(zhuǎn)角可控，而且經(jīng)緯儀上安裝有激光發(fā)射器，可以發(fā)射一條或多條激光結(jié)構(gòu)光，投射到接收器上面；接收器包括投射面板和成像單元。計(jì)算處理單元利用經(jīng)緯儀的轉(zhuǎn)角和接收器獲取的激光光斑圖像數(shù)據(jù)建立方程組，求解方程組可以獲得運(yùn)動(dòng)物體的六維位姿。本實(shí)用新型可以連續(xù)測量在較大空間中運(yùn)動(dòng)的物體，具有精度高、成本低和便于布置等優(yōu)點(diǎn)，可以替代激光跟蹤儀、坐標(biāo)測量機(jī)和室內(nèi)GPS等昂貴的位姿測量設(shè)備。
文檔編號(hào)G01C1/02GK201285280SQ200820219188
公開日2009年8月5日 申請(qǐng)日期2008年11月14日 優(yōu)先權(quán)日2008年11月14日
發(fā)明者王琛元, 田永利, 羅振軍, 鄒媛媛 申請(qǐng)人:中國科學(xué)院沈陽自動(dòng)化研究所