一種用于三維空間坐標(biāo)尋跡的數(shù)據(jù)采集裝置制造方法
【專利摘要】本實(shí)用新型公開了一種用于三維空間坐標(biāo)尋跡的數(shù)據(jù)采集裝置，本實(shí)用新型的數(shù)據(jù)采集裝置通過設(shè)置指引激光以及在受測對(duì)象上安裝平面示蹤板和圖像傳感器，使得指引激光在平面示蹤板形成三個(gè)示蹤點(diǎn)，從而能夠記錄下包含有受測對(duì)象三維運(yùn)動(dòng)姿態(tài)信息的所述各個(gè)示蹤點(diǎn)在平面示蹤板上的二維運(yùn)動(dòng)軌跡。
【專利說明】一種用于三維空間坐標(biāo)尋跡的數(shù)據(jù)采集裝置
【技術(shù)領(lǐng)域】
[0001]本實(shí)用新型涉及一種用于三維空間坐標(biāo)尋跡的數(shù)據(jù)采集裝置。
【背景技術(shù)】
[0002]目前的目標(biāo)循跡方法包含獨(dú)立和非獨(dú)立兩種。在測繪領(lǐng)域一般采用非獨(dú)立式依靠外部基準(zhǔn)進(jìn)行測距定位循跡的方法，例如GPS、衛(wèi)星軌道測量、經(jīng)緯儀全站儀地形測繪，可以測得受測目標(biāo)的三維位置。一般在飛機(jī)及潛艇的使用上采用慣性導(dǎo)航的獨(dú)立測量方法，以獲得受測目標(biāo)的三維旋轉(zhuǎn)的四元數(shù)。
[0003]現(xiàn)有技術(shù)在中高速ms級(jí)別、中遠(yuǎn)距離百米級(jí)、高精度mm級(jí)測繪的【技術(shù)領(lǐng)域】及同時(shí)需要旋轉(zhuǎn)姿態(tài)和相對(duì)偏移的場合往往缺乏必要的技術(shù)手段。
實(shí)用新型內(nèi)容
[0004]本實(shí)用新型所要解決的技術(shù)問題是:提供一種用于三維空間坐標(biāo)尋跡的數(shù)據(jù)采集裝置，能夠記錄下包含有受測對(duì)象三維運(yùn)動(dòng)姿態(tài)信息的所述各個(gè)示蹤點(diǎn)在平面示蹤板上的二維運(yùn)動(dòng)軌跡。
[0005]解決上述技術(shù)問題，本實(shí)用新型所采用的技術(shù)方案如下:
[0006]一種用于三維空間坐標(biāo)尋跡的數(shù)據(jù)采集裝置，其特征在于:所述的數(shù)據(jù)采集裝置包括指引激光、平面示蹤板和圖像傳感器；所述指引激光設(shè)有三束相互平行且不共面的激光束，指引激光安裝在固定位置上并指向固定方向，其中，第一激光束與第二激光束所在的平面與第一激光束與第三激光束所在的平面相垂直，所述平面示蹤板安裝在受測對(duì)象上，并分別與水平面、豎直平面形成不為0°且不為90°的夾角，所述第一至第三激光束依次在平面示蹤板上形成第一至第三示蹤點(diǎn)，所述圖像傳感器安裝在受測對(duì)象上，用以記錄下包含有受測對(duì)象三維運(yùn)動(dòng)姿態(tài)信息的所述各個(gè)示蹤點(diǎn)在平面示蹤板上的二維運(yùn)動(dòng)軌跡。
[0007]作為本實(shí)用新型的一種改進(jìn)，所述第一激光束與第二激光束所在的平面平行于水平面，第一激光束與第三激光束所在的平面平行于豎直平面。
[0008]為了使得數(shù)據(jù)采集裝置的靈敏度和測量范圍的綜合實(shí)用性最高，作為本實(shí)用新型的優(yōu)選實(shí)施方式，作為本實(shí)用新型的一種改進(jìn)，所述平面示蹤板分別與水平面、豎直平面形成的夾角接近于45°。
[0009]作為本實(shí)用新型的一種改進(jìn)，所述的指引激光還設(shè)有一束或者以上所述的激光束。
[0010]作為本實(shí)用新型的一種改進(jìn)，所述的數(shù)據(jù)采集裝置還包括支架；所述指引激光通過該支架安裝在固定位置上，所述支架能夠調(diào)節(jié)指引激光的指向。
[0011]與現(xiàn)有技術(shù)相比，本實(shí)用新型具有以下有益效果:
[0012]本實(shí)用新型的數(shù)據(jù)采集裝置通過設(shè)置指引激光以及在受測對(duì)象上安裝平面示蹤板和圖像傳感器，使得指引激光在平面示蹤板形成三個(gè)示蹤點(diǎn)，從而能夠記錄下包含有受測對(duì)象三維運(yùn)動(dòng)姿態(tài)信息的所述各個(gè)示蹤點(diǎn)在平面示蹤板上的二維運(yùn)動(dòng)軌跡。
【專利附圖】
【附圖說明】
[0013]下面結(jié)合附圖和具體實(shí)施例對(duì)本實(shí)用新型作進(jìn)一步的詳細(xì)說明:
[0014]圖1為本實(shí)用新型的數(shù)據(jù)采集裝置的示意圖；
[0015]圖2為三個(gè)示蹤點(diǎn)于運(yùn)動(dòng)前時(shí)刻以及運(yùn)動(dòng)后時(shí)刻在平面示蹤板上的位置示意圖；
[0016]圖3為平面示蹤板于運(yùn)動(dòng)前時(shí)刻以及運(yùn)動(dòng)后時(shí)刻的位置示意圖；
[0017]圖4為平面示蹤板于第一次等效旋轉(zhuǎn)后的位置示意圖；
[0018]圖5為平面示蹤板于第二次等效旋轉(zhuǎn)后的位置示意圖；
[0019]圖6為平面不蹤板于等效平移后的位置不意圖；
[0020]圖7為步驟五中X、y軸平移量計(jì)算的示意圖之一；
[0021]圖8為步驟五中X、y軸平移量計(jì)算的示意圖之二 ；
[0022]圖中，Board_0-運(yùn)動(dòng)前時(shí)刻的平面示蹤板，Board_l_第一次等效旋轉(zhuǎn)后的平面不蹤板，Board_2_第二次等效旋轉(zhuǎn)后的平面不蹤板，Board_3-等效平移后的平面不蹤板，Board_4-運(yùn)動(dòng)后時(shí)刻的平面不蹤板，L_0_運(yùn)動(dòng)如時(shí)刻平面不蹤板的法線，L_l_第一次等效旋轉(zhuǎn)后的平面示蹤板的法線，L_2-第二次等效旋轉(zhuǎn)后的平面示蹤板的法線，L_3-等效平移后的平面示蹤板的法線，L_4-運(yùn)動(dòng)后時(shí)刻平面示蹤板的法線。
【具體實(shí)施方式】
[0023]如圖1所示，本實(shí)用新型的用于三維空間坐標(biāo)尋跡的數(shù)據(jù)采集裝置，包括指引激光、平面示蹤板Board和圖像傳感器；所述指引激光設(shè)有三束相互平行且不共面的激光束Laser_o, Laser_x和Laser_y,指引激光安裝在固定位置上并指向固定方向，為了使得數(shù)據(jù)采集裝置的連續(xù)測量范圍更大，該指引激光的固定指向應(yīng)該大致指向受測對(duì)象的運(yùn)動(dòng)方向，其中，第一激光束Laser_o與第二激光束Laser_x所在的平面平行于水平面,第一激光束Laser_o與第三激光束Laser_y所在的平面平行于豎直平面，以使得第一激光束Laser_ο與第二激光束Laser_x所在的平面與第一激光束Laser_o與第三激光束Laser_y所在的平面相垂直，所述平面示蹤板Board安裝在受測對(duì)象上，并分別與水平面、豎直平面形成不為O。且不為90°的夾角α和β，在該兩個(gè)夾角α和β接近于45°時(shí)，數(shù)據(jù)采集裝置的靈敏度和測量范圍的綜合實(shí)用性最高，所述第一至第三激光束Laser_o,Laser_x和Laser_Y依次在平面示蹤板Board上形成第一至第三示蹤點(diǎn)Po，Px和Py，以使得受測對(duì)象產(chǎn)生相對(duì)于X軸、y軸、z軸旋轉(zhuǎn)以及相對(duì)于X軸、y軸平移的三維運(yùn)動(dòng)姿態(tài)變化時(shí)，上述三個(gè)示蹤點(diǎn)能夠在平面示蹤板Board上產(chǎn)生相應(yīng)的運(yùn)動(dòng)軌跡變化,而上述平面示蹤板Board應(yīng)具有合適的面積，以保證在受測對(duì)象定向運(yùn)動(dòng)的一定距離內(nèi)，所有的激光束均能在平面示蹤板Board上形成示蹤點(diǎn)，所述圖像傳感器安裝在受測對(duì)象上，用以記錄下包含有受測對(duì)象三維運(yùn)動(dòng)姿態(tài)信息的所述各個(gè)示蹤點(diǎn)在平面示蹤板Board上的二維運(yùn)動(dòng)軌跡。
[0024]本實(shí)用新型的三維空間坐標(biāo)尋跡方法是基于以下原理實(shí)現(xiàn)的:
[0025]I坐標(biāo)系剛性旋轉(zhuǎn)。
[0026]基本原理:參見《慣性導(dǎo)航》秦永元295頁，原文如下:￠1
ft =COSj.θ
fi =sm-
[0027]n..θ
= sin—
2
Q
q% =SlE-
j.j2.
[0028]以q^q^q^qs構(gòu)造四元數(shù):
QQQQ
[0029]Q= +q{ i0-f i/,jo+ q:、k《}= cos? +(l1+mjo+nko)sin? ^ cos? +uRsin?
[0030]四元數(shù)Q=eos#+uRsk4描述了剛體的定點(diǎn)旋轉(zhuǎn)動(dòng)，既當(dāng)只關(guān)心b系相對(duì)于a系的
II
角度位置時(shí)，可以認(rèn)為b系是由R系經(jīng)過無中間過程的一次性等效旋轉(zhuǎn)而成的，Q包含了等效旋轉(zhuǎn)的全部信息:1^為旋轉(zhuǎn)軸和旋轉(zhuǎn)方向，Θ為轉(zhuǎn)過的角度。
[0031]其中，坐標(biāo)系是在此表現(xiàn)為剛體，即軸之間的相對(duì)位置及單位長度無論是否旋轉(zhuǎn)都保持不變。
[0032]根據(jù)以上理論，結(jié)合本案實(shí)際應(yīng)用:
[0033]真實(shí)空間有一個(gè)點(diǎn)，在某個(gè)正交參照系下，其坐標(biāo)記做:
[0034]IT1=I />’，；
[0035]而在另一個(gè)同原點(diǎn)正交參照系下，其坐標(biāo)標(biāo)記為:
n2
[0036]Γ= ry2 ;
[0037]兩個(gè)參照系是按照:以“ 1^為旋轉(zhuǎn)軸和旋轉(zhuǎn)方向，Θ為轉(zhuǎn)過的角度”重合的。
ql + ?/ι2 — — ￠, — fo*) 2(^,g3 一 v/2)
[0038]定義Cj = l(q{q2 — g0f3) q； — q\ + q； — q； l(q2q, — qnqd
‘ 2(<y,% — q()q2) 2(-- — qnq}) q； — q； — q； + f" _
[0039]則有:T1=Cl r2
[0040]2坐標(biāo)系平移。
[0041]當(dāng)坐標(biāo)系僅僅發(fā)生平移時(shí)，其原點(diǎn)的示腙點(diǎn)偏移可獨(dú)立互不影響的分解為絕對(duì)坐標(biāo)系下X和y兩個(gè)非正交的投影。并且與示蹤板與X=O平面和y=0平面交線的斜率線性。可以根據(jù)示蹤板的方程求出交線斜率解算及絕對(duì)坐標(biāo)系下原點(diǎn)的平移。
[0042]因此，本實(shí)用新型的三維空間坐標(biāo)尋跡方法中，平面示蹤板Board由受測對(duì)象運(yùn)動(dòng)前的初始時(shí)刻到受測對(duì)象運(yùn)動(dòng)后的任意時(shí)刻，其無中間過程的一次性等效旋轉(zhuǎn)可以分解為第一次等效旋轉(zhuǎn)和第二次等效旋轉(zhuǎn)，其中，第一次等效旋轉(zhuǎn)為:繞垂直于兩個(gè)法線的軸旋轉(zhuǎn)兩個(gè)法線夾角的類似合頁的旋轉(zhuǎn)；第二次等效旋轉(zhuǎn)為:為了使旋轉(zhuǎn)前后坐標(biāo)系完全重合而繞旋轉(zhuǎn)后法線旋轉(zhuǎn)角度差的類似于陀螺的旋轉(zhuǎn)。將前后兩次等效旋轉(zhuǎn)的四元數(shù)順序相乘，即能得到新舊坐標(biāo)系旋轉(zhuǎn)變換的四元數(shù)。
[0043]如圖1至圖8所示，本實(shí)用新型應(yīng)用上述數(shù)據(jù)采集裝置的三維空間坐標(biāo)尋跡方法，包括以下步驟:
[0044]步驟一:
[0045]用上述數(shù)據(jù)采集裝置對(duì)受測對(duì)象進(jìn)行測量，以獲取在受測對(duì)象的運(yùn)動(dòng)過程中所述第一至第三示蹤點(diǎn)Po、Px和Py在平面示蹤板Board上的二維運(yùn)動(dòng)軌跡，從而獲得:
[0046]I)三束激光束的三維空間方程參數(shù):
? X = O*
[0047]第一激光束Laser—ο的三維方程彳……LI)
I ?r ~~ I I.I j
{父I y *
y = U;
IO*
[0049]第三激光束Laser y的三維方程彳 τ……IJ)
I Xf — I %/.L Jr — ，
[0050]其中，Lx為第一、第二激光束Laser_o和Laser_x之間的距離，Ly為第一、第三激光束Laser」)和LaSer_y之間的距離，它們由上述數(shù)據(jù)采集裝置的指引激光在生產(chǎn)、制造和安裝時(shí)確定；
[0051]2)受測對(duì)象的運(yùn)動(dòng)前時(shí)刻即所述運(yùn)動(dòng)過程的初始時(shí)刻，所述第一至第三示蹤點(diǎn)Po、Px和Py在平面示蹤板Board上的二維坐標(biāo)依次為:
[0052]PoO= (PoO (I) ,PoO (2))；
[0053]PxO=(PxO(l), PxO (2));
[0054]Py0=(Py0(l), PyO (2));
[0055]受測對(duì)象的運(yùn)動(dòng)后時(shí)刻即所述運(yùn)動(dòng)過程的除初始時(shí)刻外的任意時(shí)刻，也就是本方法的待測量時(shí)刻，所述第一至第三示蹤點(diǎn)Po、Px和Py在平面示蹤板Board上的二維坐標(biāo)依次為:
[0056]Pon= (Pon (I), Pon (2))；
[0057]Pxn=(Pxn(l), Pxn (2))；
[0058]Pyn= (Pyn (I), Pyn (2))；
[0059]其中，為了保證測量的精確度，上述受測對(duì)象在空間三維旋轉(zhuǎn)的偏角應(yīng)在正負(fù)45度之內(nèi)；
[0060]步驟二:
[0061]將所述平面示蹤板Board由運(yùn)動(dòng)前時(shí)刻至運(yùn)動(dòng)后時(shí)刻的旋轉(zhuǎn)運(yùn)動(dòng)等效分解成第一次等效旋轉(zhuǎn)和第二次等效旋轉(zhuǎn)；
[0062]其中，參見圖4，第一次等效旋轉(zhuǎn)為:以運(yùn)動(dòng)前時(shí)刻的所述第一示蹤點(diǎn)Po作為等效旋轉(zhuǎn)點(diǎn)，將運(yùn)動(dòng)前時(shí)刻的平面示蹤板Board繞第一等效旋轉(zhuǎn)軸轉(zhuǎn)過第一等效旋轉(zhuǎn)角到達(dá)第一中間平面，使得所述第一中間平面的法線與運(yùn)動(dòng)后時(shí)刻的所述平面示蹤板Board的法線相平行，所述第一等效旋轉(zhuǎn)軸即為所述平面示蹤板Board在運(yùn)動(dòng)前、后時(shí)刻的經(jīng)過所述等效旋轉(zhuǎn)點(diǎn)的法線公垂線，第一等效旋轉(zhuǎn)角等于平面示蹤板Board在運(yùn)動(dòng)前、后時(shí)刻的法線夾角，所述第一示蹤點(diǎn)Po在第一次等效旋轉(zhuǎn)前后位置不變；
[0063]參見圖5，所述第二次等效旋轉(zhuǎn)為:將所述位于第一中間平面的平面示蹤板Board轉(zhuǎn)過第二等效旋轉(zhuǎn)角到達(dá)第二中間平面，其中所述第二等效旋轉(zhuǎn)軸為第一中間平面經(jīng)過所述等效旋轉(zhuǎn)點(diǎn)的法線，第二等效旋轉(zhuǎn)角等于第一示蹤點(diǎn)Po與第二示蹤點(diǎn)Px在第一等效旋轉(zhuǎn)后的連線與運(yùn)動(dòng)后時(shí)刻的連線的空間夾角，也等于第一示蹤點(diǎn)Po與第三示蹤點(diǎn)Py在第一等效旋轉(zhuǎn)后的連線與運(yùn)動(dòng)后時(shí)刻的連線的空間夾角，使得所述第一示蹤點(diǎn)Po與第二示蹤點(diǎn)Px在第二次等效旋轉(zhuǎn)后的連線與在運(yùn)動(dòng)后時(shí)刻的連線相平行，所述第一示蹤點(diǎn)Po與第三示蹤點(diǎn)Py在第二次等效旋轉(zhuǎn)后的連線與在運(yùn)動(dòng)后時(shí)刻的連線相平行，所述第一示蹤點(diǎn)Po在第二次等效旋轉(zhuǎn)前后位置不變；
[0064]從而:
[0065]I)計(jì)算出運(yùn)動(dòng)前時(shí)刻，所述第一示蹤點(diǎn)Po與第二示蹤點(diǎn)Px之間的距離為:
[0066]LlO=…,,,2J)
[0067]計(jì)算出運(yùn)動(dòng)前時(shí)刻，所述第一示蹤點(diǎn)Po與第三示蹤點(diǎn)Py之間的距離:
[0068]LyO= ^/(Py0( I) - Po0( I )f + (Fy0(2) - Po0(2))2......2.2)
[0069]計(jì)算出運(yùn)動(dòng)前時(shí)刻，所述平面示蹤板Board的點(diǎn)法式方程系數(shù)為:
[0070]mOk=[-1/ (tan (arcsin (Lx/LxO)))-1/ (tan (asin (Ly/Ly0)))1]......2.3)
[0071]2)計(jì)算出第一次等效旋轉(zhuǎn)后，所述第一示蹤點(diǎn)Po與第二示蹤點(diǎn)Px之間的距離為:
[0072]Lxn= ^/(Pxn(I)-Pol(I))2 + (Pxn(2)-Ροπ(2))2.....….,‘2.4}
[0073]計(jì)算出第一次等效旋轉(zhuǎn)后，所述第一示蹤點(diǎn)Po與第三示蹤點(diǎn)Py之間的距離:
[0074]Lyn= -y/(Pyn( I) - Po I (I ))2 + (Pyn(2) - Ροπ(2))2……2,5)
[0075]計(jì)算出第一次等效旋轉(zhuǎn)后，所述平面示蹤板Board的點(diǎn)法式方程系數(shù)為:
[0076]mnk=[—1/(tan(arcsin(Lx/Lxn)))-1/(tan (asin (Ly/Lyn)))1]......2.6)
[0077]說明:由于平面示蹤板Board所在平面可以表示成點(diǎn)法式方程:
[0078]A (x-xO)+B (y-y0)+C (ζ-ζ0)=0，其中第一示蹤點(diǎn)Po定義為三維原點(diǎn)。作為三維空間的原點(diǎn)，x0、y0、z0都為O,并且此點(diǎn)也在平面示蹤板Board上,滿足示蹤板平面的方程。則有在示蹤板上的本文以平面方程系數(shù)[A B C]代表平面的方程；
[0079]3)將所述等效旋轉(zhuǎn)點(diǎn)定義為三維坐標(biāo)原點(diǎn)，從而計(jì)算出:
[0080]在運(yùn)動(dòng)前時(shí)刻以及在第二次等效旋轉(zhuǎn)后，所述第一示蹤點(diǎn)Po的三維坐標(biāo)均為:
[0081]PowO=PowI=O, O, O ；
[0082]在運(yùn)動(dòng)前時(shí)刻，所述第二示蹤點(diǎn)Px和第三示蹤點(diǎn)Py的三維坐標(biāo)依次為:
[0083]PxwO= (Lx,O, _Lx*m0k(I)/mOk(3))......2.7)
[0084]PywO= (O, Ly, _Ly*m0k (2) /mOk (3))......2.8)
[0085]在第二次等效旋轉(zhuǎn)后，所述第二示蹤點(diǎn)Px和第三示蹤點(diǎn)Py的三維坐標(biāo)依次為:
[0086]Pxwn= (Lx, O, _Lx*mnk(I)/mnk(3))......2.9)
[0087]Pywn= (O, Ly, _Ly*mnk (2) /mnk (3))......2.10)
[0088]4)得出所述第一等效旋轉(zhuǎn)軸的三維矢量為:
[0089]Ail=Iimk?m0k......3.1)
[0090]5)得出所述第一次等效旋轉(zhuǎn)的轉(zhuǎn)動(dòng)角度為:
[0091]ail=-arccos ((mOk.mnk) / (I mOk I * I mnk I))......3.2)
[0092]6)計(jì)算出所述平面示蹤板Board的第一次等效旋轉(zhuǎn)的非規(guī)范化(四元數(shù)平方和不為I)旋轉(zhuǎn)四元數(shù)為:
[0093]ciIt=[cos(ai1/2)Ail*sin(ail/2)] ；......3.3)
[0094]7)對(duì)所述3.3)式進(jìn)行規(guī)范化處理，使得四元數(shù)平方和為1，得出所述所述平面示蹤板Board的第一次等效旋轉(zhuǎn)的規(guī)范化旋轉(zhuǎn)四元數(shù)為:
[0095]cil=[cos (ail/2) Ail*sin (ail/2) * (kfq) ] ；......3.4)
[0096]其中，kfq=((l-cilt(l)'2)/sum(cilt (2:4).'2)) '0.5;
[0097]8)計(jì)算出第一次等效旋轉(zhuǎn)后，所述第二、第三示蹤點(diǎn)Px和Py的三維坐標(biāo)依次為:
[0098]Pxvv I =: C1 Px w0:......4.1)
[0099]Pywl=CllPywO;…?,,4.2)
[0100]其中,G=C"；
[0101]步驟三:
[0102]計(jì)算出所述平面示蹤板Board的第二次等效旋轉(zhuǎn)的旋轉(zhuǎn)四元數(shù)，包括:
[0103]I)所述第二等效旋轉(zhuǎn)軸的三維矢量與第一中間平面的法線表達(dá)式相同，為所述
2.6)式；
[0104]2)計(jì)算出所述第二次等效旋轉(zhuǎn)的轉(zhuǎn)動(dòng)角度為:
[0105]al2=-arccos((Pxwl.Pxwn)/ (I Pxwl I * I Pxwn I));......5.1)
[0106]另外，所述第二次等效旋轉(zhuǎn)的轉(zhuǎn)動(dòng)角度還可按下式進(jìn)行計(jì)算:
[0107]al2=-arccos((Pywl.Pywn)/(|PywO|*|Pywn|));......5.2)
[0108]為了提高計(jì)算精度，可以將5.1)式和5.2)式的計(jì)算結(jié)果取平均值作為所述第二次等效旋轉(zhuǎn)的轉(zhuǎn)動(dòng)角度；
[0109]3)計(jì)算出所述平面示蹤板Board的第二次等效旋轉(zhuǎn)的非規(guī)范化旋轉(zhuǎn)四元數(shù)為:
[0110]cl2t=[cos (al2/2) mnk*sin (al2/2) ] ;......5.3)
[0111]4)對(duì)所述5.3)式進(jìn)行規(guī)范化處理，使得四元數(shù)平方和為1，得出所述所述平面示蹤板Board的第二次等效旋轉(zhuǎn)的規(guī)范化旋轉(zhuǎn)四元數(shù)為:
[0112]cl2=[cos (al2/2)mnk*sin (al2/2) * (kfq2) ]......5.4)
[0113]其中，kfq2=((l-cl2t(l)~2)/sum (cl2t (2:4).~2))~0.5 ；
[0114]步驟四:
[0115]將所述平面示蹤板Board的第一次等效旋轉(zhuǎn)和第二次等效旋轉(zhuǎn)的旋轉(zhuǎn)四元數(shù)相乘，計(jì)算得到從運(yùn)動(dòng)前時(shí)刻的平面示蹤板Board到所述第二中間平面的旋轉(zhuǎn)四元數(shù):
[0116]m=C' C12^cil c12;...,,,6)
[0117]該計(jì)算結(jié)果即為受測對(duì)象從所述運(yùn)動(dòng)前時(shí)刻到所述運(yùn)動(dòng)后時(shí)刻的旋轉(zhuǎn)四元數(shù)。
[0118]步驟五:
[0119]參見圖6至圖8,以所述第一激光束Laser_o到第二激光束Laser_x的指向作為x軸，以所述第一激光束Laser_o到第三激光束Laser_y的指向作為y軸，將所述平面示蹤板Board由運(yùn)動(dòng)前時(shí)刻至運(yùn)動(dòng)后時(shí)刻的平移運(yùn)動(dòng)等效分解成沿所述X軸和y軸的平移運(yùn)動(dòng)以及沿所述第一激光束Lasers指向的平移運(yùn)動(dòng)；從而:
[0120]I)計(jì)算出第一運(yùn)動(dòng)軌跡Pon至Pxn的斜率，即所述平面示蹤板Board沿所述x軸平移時(shí)，所述第一示蹤點(diǎn)Po在平面示蹤板Board上二維移動(dòng)軌跡的斜率:
[0121 ] kx= (Pxn (2) -Pon (2)) / (Pxn (I) -Pon (I)) ;......7.1)
[0122]計(jì)算出第二運(yùn)動(dòng)軌跡Pon至Pyn的斜率，即所述平面示蹤板Board沿所述y軸平移時(shí)，所述第一示蹤點(diǎn)Po在平面示蹤板Board上二維移動(dòng)軌跡的斜率:
[0123]ky= (Pyn (2) -Pon (2)) / (Pyn (I) -Pon (I)) ;......7.2)
[0124]2)獲得所述第一運(yùn)動(dòng)軌跡Pon至Pxn的方程系數(shù):
[0125]klxn=[kx -1 O];......7.3)
[0126]獲得所述第二運(yùn)動(dòng)軌跡Pon至Pyn的方程系數(shù):
[0127]klyn=[ky -1 O];......7.4)
[0128]說明:當(dāng)平面示蹤板Board和受測對(duì)象沿上述x軸移動(dòng)時(shí)，第一示蹤點(diǎn)Po會(huì)沿著示蹤板坐標(biāo)系內(nèi)的第一示蹤點(diǎn)Po與第二示蹤點(diǎn)Px的連線(下面簡稱Pon至Pxn直線)方向移動(dòng)。由此可以計(jì)算Pon至Pxn直線的二維斜率并和Pon組成二維點(diǎn)斜式方程。點(diǎn)斜式方程可以轉(zhuǎn)化為一般方程AXx+BXy+C=0 ;本文用三下標(biāo)數(shù)組[A B C]表示此方程。
[0129]此處斜率為kx;
[0130][A B C] = [kx -1 O]；
[0131]當(dāng)平面示蹤板Board和受測對(duì)象沿上述y軸移動(dòng)時(shí)，第一示蹤點(diǎn)Po會(huì)沿著示蹤板坐標(biāo)系內(nèi)的第一示蹤點(diǎn)Po與第三示蹤點(diǎn)Py的連線(下面簡稱Pon至Pyn直線)方向移動(dòng)。由此可以計(jì)算Pon至Pyn直線的二維斜率并和Pon組成二維點(diǎn)斜式方程。點(diǎn)斜式方程可以轉(zhuǎn)化為一般方程AXx+BXy+C=0 ;本文用三下標(biāo)數(shù)組[A B C]表示此方程。
[0132]此處斜率為ky;
[0133][A B C] = [ky -1 O]；
[0134]3)計(jì)算出第一直線PoO至py的方程系數(shù)，該第一直線PoO至py經(jīng)過所述運(yùn)動(dòng)前時(shí)刻的第一示蹤點(diǎn)PoO且平行于所述第一運(yùn)動(dòng)軌跡Pon至Pxn，其與所述第二運(yùn)動(dòng)軌跡Pon至Pyn相交于第二交點(diǎn)py:
[0135]klxno=[kx —I PoO (2) _kx*PoO (I) ] ;......8.1)
[0136]計(jì)算出第二直線PoO至px的方程系數(shù)，該第二直線PoO至px經(jīng)過所述運(yùn)動(dòng)前時(shí)刻的第一示蹤點(diǎn)PoO且平行于所述第二運(yùn)動(dòng)軌跡Pon至Pyn，其與所述第一運(yùn)動(dòng)軌跡Pon至Pxn相交于第一交點(diǎn)px:
[0137]klyno=[ky —I PoO (2) _ky*PoO (I) ] ;......8.2)
[0138]根據(jù)所述8.1)式、8.2)式以及7.3)式、7.4)式解出所述第一交點(diǎn)px和第二交點(diǎn)py的坐標(biāo)，由此計(jì)算出所述第一交點(diǎn)px到運(yùn)動(dòng)后時(shí)刻的第一示蹤點(diǎn)Pon的長度:
[0139]dx=norm (px) ;......8.3)
[0140]并計(jì)算出所述第二交點(diǎn)py到運(yùn)動(dòng)后時(shí)刻的第一示蹤點(diǎn)Pon的長度:
[0141]dy=norm(py) ;......8.4)
[0142]最后，根據(jù)所述8.3)式和2.6)計(jì)算出受測對(duì)象從所述運(yùn)動(dòng)前時(shí)刻到所述運(yùn)動(dòng)后時(shí)刻的沿X軸的平移量，并根據(jù)下式判斷受測對(duì)象在X軸上的平移方向:
[0143]af=atan (mnk (3) /mnk (2));
[0144]dy=dy*sin (af);
[0145]if py(2)<0;dy=-dy;......9.1)
[0146]根據(jù)所述8.4)式和2.6)計(jì)算出受測對(duì)象從所述運(yùn)動(dòng)前時(shí)刻到所述運(yùn)動(dòng)后時(shí)刻的沿I軸的平移量，并根據(jù)下式判斷受測對(duì)象在I軸上的平移方向:
[0147]af=atan (mnk (3)/mnk (I));
[0148]dx=dx*sin (af);
[0149]if px(l)<0;dx=-dx;......9.2)
[0150]說明:示蹤板在空間上正交的X、y三維平移，Po點(diǎn)會(huì)在示蹤板上沿Pon至Pxn和Pon至Pyn兩條非正交的直線上平移。
[0151 ] Po點(diǎn)的平移按照沿Pon Pxn和Pon Pyn方向矢量分解得到Po點(diǎn)平移在示蹤板上沿Pon Pxn和Pon Pyn方向的距離。
[0152]過PoO與Pon Pyn平行的直線與Pon Pxn的交點(diǎn)px坐標(biāo)由以下過程求出。
[0153]A(l,:)=klxn(l:2);
[0154]A (2,:)=klyno(l:2);
[0155]B=- [klxn ⑶；klyno ⑶];
[0156]px=A\B;
[0157]過PoO與Pon Pxn平行的直線與Pon Pyn的交點(diǎn)py坐標(biāo)由以下過程求出。
[0158]A(l,:)=klyn(l:2);
[0159]A (2,:)=klxno(l:2);
[0160]B=- [klyn ⑶；klxno ⑶];
[0161]py=A\B。
[0162]最后，受測對(duì)象沿z軸的平移可以通過現(xiàn)有的測量方法實(shí)現(xiàn)。
[0163]另外，本實(shí)用新型數(shù)據(jù)采集裝置的指引激光還可以增設(shè)一束或者以上所述的激光束，通過在四束或者以上的激光束中抽取其中三束，分別用上述三維空間坐標(biāo)尋跡方法進(jìn)行計(jì)算，即可通過重復(fù)測量的方式提高本實(shí)用新型獲得的受測對(duì)象的旋轉(zhuǎn)四元數(shù)以及X軸、y軸平移量的測量精確度。
[0164]本實(shí)用新型的指引激光也可以通過支架安裝在上述固定位置上，當(dāng)受測對(duì)象移動(dòng)至將近偏離指引激光，即有一個(gè)示蹤點(diǎn)將近超出平面示蹤板Board時(shí)，可以讓受測對(duì)象停止下來，通過上述支架調(diào)節(jié)指引激光的指向，使得各個(gè)示蹤點(diǎn)重新回到平面示蹤板的合適位置，并記錄下支架調(diào)節(jié)指引激光指向的方位角及俯仰角，然后再次啟動(dòng)受測對(duì)象，繼續(xù)對(duì)受測對(duì)象進(jìn)行測量，實(shí)現(xiàn)對(duì)受測對(duì)象的連續(xù)尋跡。
[0165]作為本實(shí)用新型裝置的一個(gè)應(yīng)用案例，本實(shí)用新型裝置可以使用在在先申請(qǐng)《一種基于視覺測量的隧道斷面輪廊測量裝置》的姿態(tài)循跡上，指引激光及經(jīng)緯調(diào)節(jié)支架安裝于軌道上，示蹤板安裝于《一種基于視覺測量的隧道斷面輪廊測量裝置》上。并跟隨檢測車裝置一起運(yùn)動(dòng)，通過本裝置，獲得檢測車沿著X、y、z三軸旋轉(zhuǎn)的角度和沿著X、y軸平移的位移。
[0166]本實(shí)用新型不局限與上述【具體實(shí)施方式】，根據(jù)上述內(nèi)容，按照本領(lǐng)域的普通技術(shù)知識(shí)和慣用手段，在不脫離本實(shí)用新型上述基本技術(shù)思想前提下，本實(shí)用新型還可以做出其它多種形式的等效修改、替換或變更，均落在本實(shí)用新型的保護(hù)范圍之中。例如，僅要求第一激光束Laser_o與第二激光束Laser_x所在的平面與第一激光束Laser_o與第三激光束Laser_y所在的平面相垂直,而不必平行于水平面和豎直平面。
【權(quán)利要求】
1.一種用于三維空間坐標(biāo)尋跡的數(shù)據(jù)采集裝置，其特征在于:所述的數(shù)據(jù)采集裝置包括指引激光器、平面示蹤板(Board)和圖像傳感器；所述平面示蹤板(Board)安裝在受測對(duì)象上，并分別與水平面、豎直平面形成不為O。且不為90°的夾角(α和β)，所述指引激光器能夠發(fā)出三束相互平行且不共面的激光束(Laser_o, Laser_x和Laser_y),該指引激光器安裝在固定位置上并指向固定方向，使得第一激光束(Laser_o)與第二激光束(Laser_X)所在的平面與第一激光束(Laser_o)與第三激光束(Laser_y)所在的平面相垂直，并且第一至第三激光束(Laser_o,Laser_x和Laser_y)依次在平面示蹤板(Board)上形成第一至第三示蹤點(diǎn)(Po，Px和Py)，所述圖像傳感器安裝在受測對(duì)象上，用以記錄下包含有受測對(duì)象三維運(yùn)動(dòng)姿態(tài)信息的所述各個(gè)示蹤點(diǎn)在平面示蹤板(Board)上的二維運(yùn)動(dòng)軌跡。
2.根據(jù)權(quán)利要求1所述的數(shù)據(jù)采集裝置，其特征在于:所述第一激光束(Lasers)與第二激光束(Laser_x)所在的平面平行于水平面，第一激光束(Laser_o)與第三激光束(Laser_y)所在的平面平行于豎直平面。
3.根據(jù)權(quán)利要求2所述的數(shù)據(jù)采集裝置，其特征在于:所述平面示蹤板(Board)分別與水平面、豎直平面形成的夾角(α和β)接近于45°。
4.根據(jù)權(quán)利要求1至3任意一項(xiàng)所述的數(shù)據(jù)采集裝置，其特征在于:所述的指引激光器還設(shè)有一束或者以上所述的激光束。
5.根據(jù)權(quán)利要求4所述的數(shù)據(jù)采集裝置，其特征在于:所述的數(shù)據(jù)采集裝置還包括支架；所述指引激光器通過該支架安裝在固定位置上，所述支架能夠調(diào)節(jié)指引激光器的指向。
【文檔編號(hào)】G01B11/00GK204007506SQ201320841786
【公開日】2014年12月10日 申請(qǐng)日期:2013年12月18日 優(yōu)先權(quán)日:2012年12月27日
【發(fā)明者】朱茂芝, 石磊 申請(qǐng)人:廣州市地下鐵道總公司, 廣州復(fù)旦奧特科技股份有限公司