一種基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)及方法
【專利摘要】一種基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)及方法。系統(tǒng)包括機(jī)械臂、多面體編碼點(diǎn)特征測(cè)量靶標(biāo)、機(jī)械臂末端執(zhí)行器、機(jī)械臂控制柜、測(cè)量像機(jī)和上位機(jī)。本發(fā)明提供的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)和方法采用單目視覺姿態(tài)估計(jì)以滿足大視場(chǎng)的要求，并且結(jié)構(gòu)簡單、造價(jià)低。采用多面體編碼點(diǎn)特征測(cè)量靶標(biāo)避免了復(fù)雜工業(yè)環(huán)境下光污染造成的對(duì)定位精度的影響。
【專利說明】一種基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)及方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于機(jī)械臂跟蹤測(cè)量【技術(shù)領(lǐng)域】，特別是涉及一種基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)及方法。
【背景技術(shù)】
[0002]傳統(tǒng)的工業(yè)機(jī)械臂一般采用示教或離線編程的方式對(duì)加工任務(wù)進(jìn)行路徑規(guī)劃和運(yùn)動(dòng)編程。隨著技術(shù)的發(fā)展，工業(yè)生產(chǎn)對(duì)加工質(zhì)量、加工精度、裝配效率提出了更高的要求。而利用視覺系統(tǒng)，可以實(shí)時(shí)反饋機(jī)械臂末端的精確位置信息，根據(jù)位置信息可以合理地調(diào)整機(jī)械臂的運(yùn)動(dòng)軌跡，提聞生廣效率，進(jìn)一步提聞廣品的加工質(zhì)量。如何提聞機(jī)械臂末端位置姿態(tài)檢測(cè)和跟蹤精度是目前國內(nèi)外研究的熱點(diǎn)，對(duì)機(jī)械臂末端位置姿態(tài)檢測(cè)跟蹤目前主要有以下幾種方法:基于激光跟蹤儀的視覺測(cè)量跟蹤補(bǔ)償系統(tǒng)，與跟蹤靶標(biāo)、掃描頭、位姿測(cè)頭等組成視覺伺服系統(tǒng)，實(shí)現(xiàn)工業(yè)機(jī)械臂在線監(jiān)控和測(cè)量并補(bǔ)償，該方法主要存在價(jià)格昂貴和工業(yè)環(huán)境復(fù)雜造成光路遮擋等問題?；谝曈X測(cè)程法的機(jī)械臂測(cè)量跟蹤方法，該方法通過對(duì)特征點(diǎn)提取和特征點(diǎn)向量描述，建立關(guān)聯(lián)規(guī)則的特征點(diǎn)匹配策略，最后用數(shù)學(xué)方法求解剛體運(yùn)動(dòng)方程。此方法能夠跟蹤機(jī)械臂并繪制實(shí)時(shí)的運(yùn)動(dòng)軌跡，該方法難以精確建立動(dòng)力學(xué)和運(yùn)動(dòng)狀態(tài)模型，特征點(diǎn)識(shí)別匹配受工業(yè)環(huán)境影響，誤匹配嚴(yán)重?；贗ndoor GPS的測(cè)量補(bǔ)償系統(tǒng)，主要由發(fā)射站、接收器、信號(hào)處理器以及終端計(jì)算機(jī)組成，通過跟蹤安裝在需要跟蹤定位部件上的接收器或接收器組實(shí)現(xiàn)跟蹤頭坐標(biāo)測(cè)量或跟蹤目標(biāo)位姿測(cè)量。該系統(tǒng)主要存在局部組網(wǎng)造價(jià)高，精度低，可靠性差等缺點(diǎn)。

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

[0003]為了解決上述問題，本發(fā)明的目的在于提供一種基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)及方法。
[0004]為了達(dá)到上述目的，本發(fā)明提供的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)及方法包括:機(jī)械臂、多面體編碼點(diǎn)特征測(cè)量靶標(biāo)、機(jī)械臂末端執(zhí)行器、機(jī)械臂控制柜、測(cè)量像機(jī)和上位機(jī)；其中:機(jī)械臂為電動(dòng)多關(guān)節(jié)工業(yè)機(jī)械臂，其活動(dòng)末端裝有多面體編碼點(diǎn)特征測(cè)量靶標(biāo)和機(jī)械臂末端執(zhí)行器；
[0005]多面體編碼點(diǎn)特征測(cè)量靶標(biāo)為帶有編碼特征點(diǎn)的測(cè)量識(shí)別裝置，其安裝在機(jī)械臂與機(jī)械臂末端執(zhí)行器的連接部位；
[0006]機(jī)械臂末端執(zhí)行器為機(jī)械臂的操作執(zhí)行部件，機(jī)械臂通過安裝在其末端的機(jī)械臂末端執(zhí)行器完成預(yù)定動(dòng)作；
[0007]機(jī)械臂控制柜為工業(yè)機(jī)械臂的驅(qū)動(dòng)控制部件，其與機(jī)械臂相連接，用于控制機(jī)械
臂工作；
[0008]測(cè)量像機(jī)為單目CXD圖像采集裝置，其與上位機(jī)相連接，用于采集整個(gè)操作區(qū)的圖像信息，并傳送給上位機(jī)；
[0009]上位機(jī)為控制計(jì)算機(jī)，為本系統(tǒng)的控制核心；其與機(jī)械臂控制柜相連接。[0010]所述的多面體編碼點(diǎn)特征測(cè)量靶標(biāo)為多面體編碼方位裝置，其為一多面體形狀，其中軸部位設(shè)有連接軸，連接軸的兩端分別與機(jī)械臂的末端和機(jī)械臂末端執(zhí)行器相連接；多面體編碼點(diǎn)特征測(cè)量靶標(biāo)上具有多個(gè)矩形側(cè)面，每一側(cè)面構(gòu)成一個(gè)測(cè)量面，每個(gè)測(cè)量面的表面粘貼有一至多個(gè)編碼特征點(diǎn)；每個(gè)編碼特征點(diǎn)包含個(gè)相對(duì)位置固定的特征位和3個(gè)編碼位。
[0011]所述的編碼特征點(diǎn)采用黑色背景，在黑色背景上根據(jù)排列規(guī)則粘貼由高反射率反光材料制成的特征位和編碼位。
[0012]本發(fā)明提供的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)的跟蹤測(cè)量方法包括按順序執(zhí)行的下列步驟:
[0013]步驟一、工作空間整體布局和裝置固定的SOl階段:根據(jù)機(jī)械臂的運(yùn)動(dòng)軌跡、工作范圍，將測(cè)量像機(jī)固定在待加工工件上；
[0014]步驟二、測(cè)量像機(jī)與工件的預(yù)標(biāo)定、建立靶標(biāo)和末端執(zhí)行器校準(zhǔn)數(shù)據(jù)庫的S02階段:將工件空間坐標(biāo)預(yù)先校準(zhǔn)，經(jīng)工件工裝定位與預(yù)定標(biāo)，像機(jī)坐標(biāo)系(OCXCYCZC)在工件坐標(biāo)系(0WXWYWZW)下的位姿(TCW, RCW)為固定值且已知；通過對(duì)基準(zhǔn)校準(zhǔn)球進(jìn)行校準(zhǔn)來確定機(jī)械臂末端執(zhí)行器的位置在靶標(biāo)坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換關(guān)系，從而確定末端執(zhí)行器坐標(biāo)系(ODXDYDZD)在靶標(biāo)坐標(biāo)系(OEXEYEZE)下的位姿(TDE, RDE)；
[0015]步驟三、工作空間坐標(biāo)系建立的S03階段:在機(jī)械臂基座處建立機(jī)械臂坐標(biāo)系(ORXRYRZR);靶標(biāo)坐標(biāo)系(OEXEYEZE)在編碼特征點(diǎn)或靶標(biāo)其他位置上選?。荒┒藞?zhí)行器坐標(biāo)系(ODXDYDZD)建立在機(jī)械臂末端執(zhí)彳了器的末端；
[0016]步驟四、機(jī)械臂到達(dá)預(yù)定位置的S04階段:通過上位機(jī)輸入目標(biāo)空間坐標(biāo)參數(shù)，利用機(jī)械臂控制柜控制機(jī)械臂到達(dá)預(yù)定位置；
[0017]步驟五、靶標(biāo)實(shí)時(shí)成像測(cè)量的S05階段:測(cè)量像機(jī)對(duì)安裝在機(jī)械臂與機(jī)械臂末端執(zhí)行器之間的多面體編碼點(diǎn)特征測(cè)量靶標(biāo)進(jìn)行實(shí)時(shí)成像測(cè)量，然后將圖像數(shù)據(jù)傳遞到上位機(jī)；
[0018]步驟六、獲取機(jī)械臂末端執(zhí)行器姿態(tài)的S06階段:上位機(jī)對(duì)上述測(cè)量圖像經(jīng)過編碼特征點(diǎn)掃描、質(zhì)心定位、編碼特征點(diǎn)識(shí)別，后方交會(huì)求解像機(jī)外方位和相對(duì)方位，利用外極線約束外極角匹配方法進(jìn)行編碼特征點(diǎn)匹配，基于共線性約束的優(yōu)化平差求解編碼特征點(diǎn)空間三維坐標(biāo)；像機(jī)基站成像中利用空間存在多于點(diǎn)的相互之間空間距離已知的特征點(diǎn)與成像點(diǎn)之間的關(guān)系，采用基于空間共角約束條件下的交會(huì)姿態(tài)估計(jì)算法和多外部方位裝置優(yōu)化確定像機(jī)基站的絕對(duì)外部方位(TCW，RCW)，即像機(jī)坐標(biāo)系(OCXCYCZC)在工件坐標(biāo)系(0WXWYWZW)下的位姿(TCW，RCff);通過坐標(biāo)逆變換獲得靶標(biāo)坐標(biāo)系在像機(jī)坐標(biāo)系(OCXCYCZC)下的位姿(TEC，REC)；
[0019]步驟七、獲取機(jī)械臂末端執(zhí)行器精確空間坐標(biāo)的S07階段:利用機(jī)械臂末端執(zhí)行器與多面體編碼點(diǎn)特征測(cè)量靶標(biāo)之間的剛體連接關(guān)系和預(yù)校準(zhǔn)并建立的末端執(zhí)行器數(shù)據(jù)庫，最終獲得機(jī)械臂末端執(zhí)行器在工件坐標(biāo)系下的精確坐標(biāo)位置及姿態(tài)；
[0020]步驟八、補(bǔ)償機(jī)械臂末端執(zhí)行器位姿偏差的S08階段:通過位姿檢測(cè)補(bǔ)償系統(tǒng)計(jì)算程序位姿和測(cè)量位姿偏差值，由上位機(jī)發(fā)送補(bǔ)償位移給機(jī)械臂控制柜，從而精確控制機(jī)械臂的動(dòng)作；
[0021]步驟九、判斷預(yù)定動(dòng)作是否完成的S09階段:判斷本次操作的預(yù)定動(dòng)作是否全部完成，如果判斷結(jié)果為“是”，則本操作流程結(jié)束，否則下一步返回S03階段的入口處，繼續(xù)
下一步動(dòng)作。
[0022]由于工業(yè)機(jī)械臂裝配現(xiàn)場(chǎng)環(huán)境復(fù)雜，加工精度高，本發(fā)明提供的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)和方法采用單目視覺姿態(tài)估計(jì)以滿足大視場(chǎng)的要求，并且結(jié)構(gòu)簡單、造價(jià)低。采用多面體編碼點(diǎn)特征測(cè)量靶標(biāo)避免了復(fù)雜工業(yè)環(huán)境下光污染造成的對(duì)定位精度的影響。
[0023]本發(fā)明與已有技術(shù)相比具有以下特點(diǎn):
[0024]1.提出基于單目視覺姿態(tài)估計(jì)算法和多面體編碼點(diǎn)特征靶標(biāo)的機(jī)械臂末端跟蹤測(cè)量方法，單目視覺測(cè)量解決了現(xiàn)場(chǎng)空間的限制，可實(shí)現(xiàn)復(fù)雜現(xiàn)場(chǎng)環(huán)境下的實(shí)時(shí)跟蹤測(cè)量。
[0025]2.采用多面體編碼點(diǎn)測(cè)量靶標(biāo)作為測(cè)量靶標(biāo)，多面體結(jié)構(gòu)能夠確保機(jī)械臂任意位姿均在單主測(cè)面或雙主側(cè)面出現(xiàn)，從而實(shí)現(xiàn)全工況實(shí)時(shí)監(jiān)測(cè)。
[0026]3.多面體編碼點(diǎn)特征測(cè)量靶標(biāo)的每一測(cè)量面上均包含I到多個(gè)編碼特征點(diǎn)，從而能夠確保編碼特征點(diǎn)具有唯一識(shí)別特性，提高了測(cè)量的穩(wěn)定性和可靠性。另外，主側(cè)面上多個(gè)編碼特征點(diǎn)增加了控制點(diǎn)的數(shù)量，編碼特征點(diǎn)上特征位之間的固定約束可使控制點(diǎn)在位姿估計(jì)解算過程中相互校準(zhǔn)，可在提高三維不確定度指標(biāo)的同時(shí)進(jìn)一步消除偽特征像點(diǎn)造成的粗大誤差。
【專利附圖】

【附圖說明】
[0027]圖1為本發(fā)明提供的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)結(jié)構(gòu)示意圖。
[0028]圖2為本發(fā)明提供的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)中多面體編碼點(diǎn)特征測(cè)量祀標(biāo)結(jié)構(gòu)示意圖。
[0029]圖3為本發(fā)明提供的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)中各坐標(biāo)系示意圖。
[0030]圖4為采用本發(fā)明提供的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量方法時(shí)通過基準(zhǔn)校準(zhǔn)球校準(zhǔn)機(jī)械臂末端執(zhí)行器過程示意圖。
[0031]圖5為本發(fā)明提供的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量方法流程圖。【具體實(shí)施方式】
[0032]下面結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明提供的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)及方法進(jìn)行詳細(xì)說明。
[0033]如圖1所示，本發(fā)明提供的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)包括:機(jī)械臂1、多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2、機(jī)械臂末端執(zhí)行器3、機(jī)械臂控制柜4、測(cè)量像機(jī)5和上位機(jī)6 ;其中:機(jī)械臂I為電動(dòng)多關(guān)節(jié)工業(yè)機(jī)械臂，其活動(dòng)末端裝有多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2和機(jī)械臂末端執(zhí)行器3 ；
[0034]多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2為帶有編碼特征點(diǎn)的測(cè)量識(shí)別裝置，其安裝在機(jī)械臂I與機(jī)械臂末端執(zhí)行器3的連接部位；
[0035]機(jī)械臂末端執(zhí)行器3為機(jī)械臂I的操作執(zhí)行部件，機(jī)械臂I通過安裝在其末端的機(jī)械臂末端執(zhí)行器3完成預(yù)定動(dòng)作；
[0036]機(jī)械臂控制柜4為工業(yè)機(jī)械臂I的驅(qū)動(dòng)控制部件，其與機(jī)械臂I相連接，用于控制機(jī)械臂I工作；
[0037]測(cè)量像機(jī)5為單目CXD圖像采集裝置，其與上位機(jī)6相連接，用于采集整個(gè)操作區(qū)的圖像信息，并傳送給上位機(jī)6 ；
[0038]上位機(jī)6為控制計(jì)算機(jī)，為本系統(tǒng)的控制核心；其與機(jī)械臂控制柜4相連接。
[0039]如圖2所示，所述的多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2為多面體編碼方位裝置，其為一多面體形狀，其中軸部位設(shè)有連接軸123，連接軸123的兩端分別與機(jī)械臂I的末端和機(jī)械臂末端執(zhí)行器3相連接。多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2上具有多個(gè)矩形側(cè)面，每一側(cè)面構(gòu)成一個(gè)測(cè)量面，測(cè)量面的個(gè)數(shù)需根據(jù)所使用的機(jī)械臂I的尺寸確定(如圖2采用八面體結(jié)構(gòu))，每個(gè)測(cè)量面的表面粘貼有一至多個(gè)編碼特征點(diǎn)(圖2所示的為兩個(gè)編碼特征點(diǎn))；每個(gè)編碼特征點(diǎn)包含5個(gè)相對(duì)位置固定的特征位121和3個(gè)編碼位120，為了易于識(shí)別和解算，各個(gè)測(cè)量面上編碼特征點(diǎn)的標(biāo)號(hào)應(yīng)各不相同，以避免編碼特征點(diǎn)解算時(shí)出現(xiàn)誤匹配現(xiàn)象；每一個(gè)編碼特征點(diǎn)均構(gòu)成外部方位裝置，主側(cè)面上多個(gè)編碼特征點(diǎn)增加了控制點(diǎn)的數(shù)量，編碼特征點(diǎn)上特征位之間的固定約束可使控制點(diǎn)在位姿估計(jì)解算過程中相互校準(zhǔn)，利用空間多于3點(diǎn)的相互之間空間距離已知的特征點(diǎn)與之成像點(diǎn)之間的關(guān)系即可確定像機(jī)坐標(biāo)系相對(duì)于靶標(biāo)坐標(biāo)系之間的方位關(guān)系。
[0040]所述的編碼特征點(diǎn)采用黑色背景，在黑色背景上根據(jù)排列規(guī)則粘貼由高反射率反光材料制成的特征位121和編碼位120。
[0041]所述的多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2的外形可根據(jù)實(shí)際情況進(jìn)行設(shè)計(jì)，并應(yīng)保證機(jī)械臂I和機(jī)械臂末端執(zhí)行器3之間剛性連接；同時(shí)應(yīng)合理設(shè)計(jì)多棱柱體體積，使其不會(huì)影響機(jī)械臂I末端負(fù)重比。
[0042]機(jī)械臂末端執(zhí)行器坐標(biāo)系與機(jī)械臂基座坐標(biāo)系之間的坐標(biāo)變換關(guān)系由機(jī)械臂I的正向運(yùn)動(dòng)學(xué)模型及各關(guān)節(jié)變量值獲得，由于不存在完美工業(yè)機(jī)械臂運(yùn)動(dòng)學(xué)模型，并且機(jī)械加工及安裝也存在較大誤差，因此需要外部測(cè)量系統(tǒng)對(duì)機(jī)械臂I進(jìn)行校準(zhǔn)和補(bǔ)償。測(cè)量像機(jī)5應(yīng)與待加工工件7處在同一工作空間內(nèi)且保證工件7和機(jī)械臂I處于測(cè)量像機(jī)5的視場(chǎng)范圍內(nèi)，測(cè)量像機(jī)5位置固定后，經(jīng)工件7工裝定位和預(yù)標(biāo)定，測(cè)量像機(jī)5的像機(jī)坐標(biāo)系在工件坐標(biāo)系下的位姿為固定值。測(cè)量像機(jī)5的內(nèi)部方位參數(shù)、有效焦距、像機(jī)原點(diǎn)坐標(biāo)、像機(jī)畸變參數(shù)等都會(huì)直接影響單目視覺姿態(tài)估計(jì)解算結(jié)果，從而影響在工件坐標(biāo)系下多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2上編碼特征點(diǎn)解算，結(jié)果直接影響定位精度，因此測(cè)量像機(jī)5必須要經(jīng)過像機(jī)內(nèi)部參數(shù)高精度校準(zhǔn)；測(cè)量像機(jī)5將利用調(diào)整光圈大小來設(shè)置曝光時(shí)間，并將采集到的圖像傳送給上位機(jī)6。上位機(jī)6對(duì)采集到的圖像進(jìn)行數(shù)據(jù)處理，之后掃描采集圖像并進(jìn)行質(zhì)心定位，對(duì)編碼特征點(diǎn)進(jìn)行識(shí)別，后方交會(huì)求解像機(jī)外方位及相對(duì)方位，經(jīng)過前方交會(huì)獲取編碼特征點(diǎn)三維坐標(biāo)，通過優(yōu)化平差精確求解編碼特征點(diǎn)三維坐標(biāo)，經(jīng)過坐標(biāo)系轉(zhuǎn)換，確定機(jī)械臂I末端在工件坐標(biāo)系下的位置姿態(tài)，與已經(jīng)標(biāo)定的工件坐標(biāo)求取偏差，最后利用機(jī)械臂控制柜4對(duì)機(jī)械臂I末端進(jìn)行姿態(tài)位置補(bǔ)償。另外，多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2固定在機(jī)械臂I的末端，應(yīng)保證剛性連接，以避免工作過程中的不穩(wěn)定性，多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2的尺寸設(shè)計(jì)還要保證機(jī)械臂I末端不能超過額定負(fù)重比。根據(jù)機(jī)械臂運(yùn)動(dòng)學(xué)理論合理設(shè)計(jì)多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2布局對(duì)提高機(jī)械臂I末端定位精度具有直接作用。
[0043]如圖4所示，所述的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)還包括基準(zhǔn)校準(zhǔn)球8 ;基準(zhǔn)校準(zhǔn)球8為基準(zhǔn)校準(zhǔn)裝置，一般由碳化硅材料制成，球體表面具有較高的精度及表面清潔度，并且在球體表面設(shè)置具有一定尺寸精度及光潔度的孔作為基準(zhǔn)孔。在本測(cè)量系統(tǒng)的測(cè)量范圍內(nèi)安裝基準(zhǔn)校準(zhǔn)球8，安裝后其位置始終保持不變。利用測(cè)量像機(jī)5對(duì)同一基準(zhǔn)孔分別進(jìn)行機(jī)械臂I不同位姿成像的測(cè)量，并將像機(jī)坐標(biāo)系下的坐標(biāo)值通過坐標(biāo)變換轉(zhuǎn)換到工件坐標(biāo)下。通過機(jī)械臂I對(duì)設(shè)置在測(cè)量現(xiàn)場(chǎng)的基準(zhǔn)校準(zhǔn)球8某一固定點(diǎn)進(jìn)行多次不同位姿成像的測(cè)量，并通過多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2的空間約束可以求解出測(cè)量像機(jī)5到多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2之間的空間位置關(guān)系，通過球面擬合，獲得機(jī)械臂末端執(zhí)行器3的位置在靶標(biāo)坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換關(guān)系。
[0044]如圖5所示，本發(fā)明提供的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量方法包括按順序執(zhí)行的下列步驟:
[0045]步驟一、工作空間整體布局和裝置固定的SOl階段:應(yīng)選取震動(dòng)強(qiáng)度小的工作環(huán)境，合理規(guī)劃測(cè)量布局及測(cè)量網(wǎng)絡(luò)，以保證測(cè)量精度。根據(jù)機(jī)械臂I的運(yùn)動(dòng)軌跡、工作范圍，合理設(shè)置機(jī)械臂I與待加工工件7的工作布局，將測(cè)量像機(jī)5固定在待加工工件7上，其安裝應(yīng)保證多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2與測(cè)量像機(jī)5之間具有良好的交會(huì)角度。
[0046]步驟二、測(cè)量像機(jī)與工件的預(yù)標(biāo)定、建立靶標(biāo)和末端執(zhí)行器校準(zhǔn)數(shù)據(jù)庫的S02階段:將工件空間坐標(biāo)預(yù)先校準(zhǔn)，經(jīng)工件工裝定位與預(yù)定標(biāo)，像機(jī)坐標(biāo)系(OCXCYCZC)在工件坐標(biāo)系(0WXWYWZW)T的位姿(TCW，RCW)為固定值且已知。由于機(jī)械臂末端執(zhí)行器3與多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2為剛體連接且定標(biāo)，如圖4所示，通過對(duì)基準(zhǔn)校準(zhǔn)球8進(jìn)行校準(zhǔn)可以確定機(jī)械臂末端執(zhí)行器3的位置在靶標(biāo)坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換關(guān)系，從而確定末端執(zhí)行器坐標(biāo)系(ODXDYDZD)在靶標(biāo)坐標(biāo)系(OEXEYEZE)下的位姿(TDE, RDE)。
[0047]步驟三、工作空間坐標(biāo)系建立的S03階段:如圖3所示，由于機(jī)械臂I基座是固定的，所以在機(jī)械臂I基座處建立機(jī)械臂坐標(biāo)系(ORXRYRZR)。測(cè)量像機(jī)5位置固定后，像機(jī)坐標(biāo)系(OCXCYCZC)和工件坐標(biāo)系(0WXWYWZW)的建立可根據(jù)工件工裝定位與預(yù)定標(biāo)確定，靶標(biāo)坐標(biāo)系(OEXEYEZE)可在編碼特征點(diǎn)上選取，也可選在靶標(biāo)其他位置，因?yàn)橐曈X測(cè)量中的空間剛體變換建立了物方空間內(nèi)不同坐標(biāo)系間的轉(zhuǎn)換關(guān)系，通過空間剛體變換所建立的坐標(biāo)系相對(duì)位姿關(guān)系可以用旋轉(zhuǎn)矩陣和平移矩陣來描述。末端執(zhí)行器坐標(biāo)系(ODXDYDZD)建立在機(jī)械臂末端執(zhí)行器3的末端。
[0048]步驟四、機(jī)械臂到達(dá)預(yù)定位置的S04階段:由于工件坐標(biāo)預(yù)先校準(zhǔn)已知，首先通過上位機(jī)6輸入目標(biāo)空間坐標(biāo)參數(shù)，利用機(jī)械臂控制柜4控制機(jī)械臂I到達(dá)預(yù)定位置；
[0049]步驟五、靶標(biāo)實(shí)時(shí)成像測(cè)量的S05階段:測(cè)量像機(jī)5對(duì)安裝在機(jī)械臂I與機(jī)械臂末端執(zhí)行器3之間的多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2進(jìn)行實(shí)時(shí)成像測(cè)量，然后將圖像數(shù)據(jù)傳遞到上位機(jī)6 ；
[0050]步驟六、獲取機(jī)械臂末端執(zhí)行器姿態(tài)的S06階段:上位機(jī)6對(duì)上述測(cè)量圖像經(jīng)過編碼特征點(diǎn)掃描、質(zhì)心定位、編碼特征點(diǎn)識(shí)別，后方交會(huì)求解像機(jī)外方位和相對(duì)方位，利用外極線約束外極角匹配方法進(jìn)行編碼特征點(diǎn)匹配?；诠簿€性約束的優(yōu)化平差求解編碼特征點(diǎn)空間三維坐標(biāo)。像機(jī)基站成像中利用空間存在多于3點(diǎn)的相互之間空間距離已知的特征點(diǎn)與成像點(diǎn)之間的關(guān)系，采用基于空間共角約束條件下的交會(huì)姿態(tài)估計(jì)算法和多外部方位裝置優(yōu)化可以確定像機(jī)基站的絕對(duì)外部方位(TCW，RCff),即像機(jī)坐標(biāo)系(OCXCYCZC)在工件坐標(biāo)系(0WXWYWZW)下的位姿(TCW，RCW)。通過坐標(biāo)逆變換則可獲得靶標(biāo)坐標(biāo)系在像機(jī)坐標(biāo)系(OCXCYCZC)下的位姿(TEC，REC)。由于像機(jī)坐標(biāo)系在工件坐標(biāo)系下的位姿已經(jīng)預(yù)先標(biāo)定且已知，通過坐標(biāo)變換可以獲得機(jī)械臂末端的靶標(biāo)坐標(biāo)系在工件坐標(biāo)系下的位姿(TEW, REDo
[0051]由于機(jī)械臂末端執(zhí)行器3與多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2可作為剛體連接且定標(biāo)，如圖4所示，通過對(duì)基準(zhǔn)校準(zhǔn)球8校準(zhǔn)可以確定機(jī)械臂末端執(zhí)行器3在靶標(biāo)坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換關(guān)系，因此末端執(zhí)行器坐標(biāo)系(ODXDYDZD)在靶標(biāo)坐標(biāo)系(OEXEYEZE)下的位姿(TDE，RDE)確定。同樣經(jīng)靶標(biāo)坐標(biāo)系(OEXEYEZE)、像機(jī)坐標(biāo)系(OCXCYCZC)轉(zhuǎn)換至工件坐標(biāo)系(0WXWYWZW)，該位姿(TDW，RDW)即確定了測(cè)量時(shí)刻的機(jī)械臂末端執(zhí)行器3的位置及姿態(tài)。
[0052]步驟七、獲取機(jī)械臂末端執(zhí)行器精確空間坐標(biāo)的S07階段:利用機(jī)械臂末端執(zhí)行器3與多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2之間的剛體連接關(guān)系和預(yù)校準(zhǔn)并建立的末端執(zhí)行器數(shù)據(jù)庫，最終獲得機(jī)械臂末端執(zhí)行器3在工件坐標(biāo)系下的精確坐標(biāo)位置及姿態(tài)；
[0053]步驟八、補(bǔ)償機(jī)械臂末端執(zhí)行器位姿偏差的S08階段:通過位姿檢測(cè)補(bǔ)償系統(tǒng)計(jì)算程序位姿和測(cè)量位姿偏差值，由上位機(jī)6發(fā)送補(bǔ)償位移給機(jī)械臂控制柜4，從而精確控制機(jī)械臂I的動(dòng)作；
[0054]步驟九、判斷預(yù)定動(dòng)作是否完成的S09階段:判斷本次操作的預(yù)定動(dòng)作是否全部完成，如果判斷結(jié)果為“是”，則本操作流程結(jié)束，否則下一步返回S03階段的入口處，繼續(xù)
下一步動(dòng)作。
[0055]本跟蹤測(cè)量系統(tǒng)的工作原理:
[0056]利用上位機(jī)6通過機(jī)械臂控制柜4控制機(jī)械臂I到達(dá)預(yù)定位置，然后利用測(cè)量像機(jī)5對(duì)安裝在機(jī)械臂I與機(jī)械臂末端執(zhí)行器3之間的多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2進(jìn)行實(shí)時(shí)成像測(cè)量，然后將圖像數(shù)據(jù)傳遞到上位機(jī)6，上位機(jī)6對(duì)獲得的圖像進(jìn)行編碼特征點(diǎn)解算，并根據(jù)編碼特征點(diǎn)之間的已知空間坐標(biāo)約束利用姿態(tài)估計(jì)方法獲得靶標(biāo)坐標(biāo)系與像機(jī)坐標(biāo)系之間的相對(duì)位姿，利用機(jī)械臂末端執(zhí)行器3與多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2之間的剛體連接關(guān)系和預(yù)校準(zhǔn)并建立的末端執(zhí)行器數(shù)據(jù)庫，最終獲得機(jī)械臂末端執(zhí)行器3在工件坐標(biāo)系下的精確坐標(biāo)位置及姿態(tài)。通過位姿檢測(cè)補(bǔ)償系統(tǒng)計(jì)算程序位姿和測(cè)量位姿偏差值，從而達(dá)到精確控制機(jī)械臂I動(dòng)作的目的。
[0057]對(duì)工業(yè)機(jī)械臂運(yùn)動(dòng)學(xué)分析，確定其運(yùn)動(dòng)軌跡優(yōu)化、工作空間、誤差補(bǔ)償模型，合理設(shè)計(jì)像機(jī)與被測(cè)工件距離，確定測(cè)量布局優(yōu)化。在此基礎(chǔ)上合理設(shè)計(jì)多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2與測(cè)量像機(jī)5布局優(yōu)化，從而獲得工作空間高精度全局實(shí)時(shí)測(cè)量。另外，由于機(jī)械臂裝配現(xiàn)場(chǎng)環(huán)境復(fù)雜，測(cè)量視場(chǎng)范圍大，實(shí)時(shí)性要求高，本發(fā)明采用單目視覺姿態(tài)估計(jì)方法結(jié)合多面體編碼特征測(cè)量靶標(biāo)2進(jìn)行機(jī)械臂I的位姿補(bǔ)償測(cè)量。
[0058]測(cè)量現(xiàn)場(chǎng)環(huán)境的光污染決定了編碼特征點(diǎn)具有唯一識(shí)別特性，本發(fā)明以多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2作為測(cè)量靶標(biāo)，可避免編碼特征點(diǎn)的誤識(shí)別引起測(cè)量粗大誤差。采用多面體結(jié)構(gòu)能夠確保機(jī)械臂I在任意工作位姿均有在單目視覺三維不確定度指標(biāo)滿足范圍內(nèi)的單主測(cè)面或雙主側(cè)面出現(xiàn)，實(shí)現(xiàn)全工況測(cè)量。
[0059]通過識(shí)別編碼方位裝置確定測(cè)量像機(jī)5與多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2的位姿轉(zhuǎn)換關(guān)系。多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2上每一個(gè)編碼特征點(diǎn)包含相對(duì)位置固定且已知的5個(gè)特征位121和3個(gè)編碼位120，每一個(gè)編碼特征點(diǎn)均構(gòu)成外部方位裝置，主側(cè)面上多個(gè)編碼特征點(diǎn)增加了控制點(diǎn)的數(shù)量，編碼特征點(diǎn)上特征位之間的固定約束使控制點(diǎn)在位姿估計(jì)解算過程中互校準(zhǔn)，在提高三維不確定度指標(biāo)的同時(shí)，進(jìn)一步消除了偽特征像點(diǎn)造成的粗大測(cè)量誤差的可能。
[0060] 多面體編碼點(diǎn)特征測(cè)量靶標(biāo)2的每一測(cè)量面上均包含I到多個(gè)編碼特征點(diǎn)，由于每一編碼特征點(diǎn)的唯一性，在特征像點(diǎn)聚類分析及編碼解碼中，編碼特征點(diǎn)區(qū)域中和外部的偽特征像點(diǎn)對(duì)其均不會(huì)造成影響，因此可以確保測(cè)量的穩(wěn)定性和可靠性。另外，為了易于識(shí)別和解算編碼特征點(diǎn)布局，可采用每一主測(cè)面和相鄰測(cè)面大小不同，各個(gè)測(cè)量面上編碼特征點(diǎn)編號(hào)不同的方式。
【權(quán)利要求】
1.一種基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)，其特征在于:其包括:機(jī)械臂(1)、多面體編碼點(diǎn)特征測(cè)量靶標(biāo)(2)、機(jī)械臂末端執(zhí)行器(3)、機(jī)械臂控制柜(4)、測(cè)量像機(jī)(5)和上位機(jī)(6);其中:機(jī)械臂(I)為電動(dòng)多關(guān)節(jié)工業(yè)機(jī)械臂，其活動(dòng)末端裝有多面體編碼點(diǎn)特征測(cè)量靶標(biāo)(2)和機(jī)械臂末端執(zhí)行器(3)； 多面體編碼點(diǎn)特征測(cè)量靶標(biāo)(2)為帶有編碼特征點(diǎn)的測(cè)量識(shí)別裝置，其安裝在機(jī)械臂(1)與機(jī)械臂末端執(zhí)行器(3)的連接部位； 機(jī)械臂末端執(zhí)行器(3 )為機(jī)械臂(I)的操作執(zhí)行部件，機(jī)械臂(I)通過安裝在其末端的機(jī)械臂末端執(zhí)行器(3)完成預(yù)定動(dòng)作； 機(jī)械臂控制柜(4)為工業(yè)機(jī)械臂(I)的驅(qū)動(dòng)控制部件，其與機(jī)械臂(I)相連接，用于控制機(jī)械臂(I)工作； 測(cè)量像機(jī)(5)為單目CXD圖像采集裝置，其與上位機(jī)(6)相連接，用于采集整個(gè)操作區(qū)的圖像信息，并傳送給上位機(jī)(6)； 上位機(jī)(6)為控制計(jì)算機(jī)，為本系統(tǒng)的控制核心；其與機(jī)械臂控制柜(4)相連接。
2.根據(jù)權(quán)利要求1所述的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)，其特征在于:所述的多面體編碼點(diǎn)特征測(cè)量靶標(biāo)(2)為多面體編碼方位裝置，其為一多面體形狀，其中軸部位設(shè)有連接軸(123)，連接軸(123)的兩端分別與機(jī)械臂(I)的末端和機(jī)械臂末端執(zhí)行器(3)相連接；多面體編碼點(diǎn)特征測(cè)量靶標(biāo)(2)上具有多個(gè)矩形側(cè)面，每一側(cè)面構(gòu)成一個(gè)測(cè)量面，每個(gè)測(cè)量面的表面粘貼有一至多個(gè)編碼特征點(diǎn)；每個(gè)編碼特征點(diǎn)包含(5)個(gè)相對(duì)位置固定的特征位(121)和3個(gè)編碼位(120)。
3.根據(jù)權(quán)利要求2所述的基于編碼方位裝置機(jī)械臂末端跟蹤測(cè)量系統(tǒng)，其特征在于:所述的編碼特征點(diǎn)采用黑色背景，在黑色背景上根據(jù)排列規(guī)則粘貼由高反射率反光材料制成的特征位(121)和編碼位(120)。
4.一種如權(quán)利要求1所述的基于編碼方位裝置的機(jī)械臂末端跟蹤測(cè)量系統(tǒng)的跟蹤測(cè)量方法，其特征在于:所述的測(cè)量方法包括按順序執(zhí)行的下列步驟: 步驟一、工作空間整體布局和裝置固定的SOl階段:根據(jù)機(jī)械臂(I)的運(yùn)動(dòng)軌跡、工作范圍，將測(cè)量像機(jī)(5)固定在待加工工件(7)上； 步驟二、測(cè)量像機(jī)與工件的預(yù)標(biāo)定、建立靶標(biāo)和末端執(zhí)行器校準(zhǔn)數(shù)據(jù)庫的S02階段:將工件空間坐標(biāo)預(yù)先校準(zhǔn)，經(jīng)工件工裝定位與預(yù)定標(biāo)，像機(jī)坐標(biāo)系(OCXCYCZC)在工件坐標(biāo)系(OWXWYWZW)下的位姿(TCW，RCff)為固定值且已知；通過對(duì)基準(zhǔn)校準(zhǔn)球(8)進(jìn)行校準(zhǔn)來確定機(jī)械臂末端執(zhí)行器(3)的位置在靶標(biāo)坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換關(guān)系，從而確定末端執(zhí)行器坐標(biāo)系(ODXDYDZD)在靶標(biāo)坐標(biāo)系(OEXEYEZE)下的位姿(TDE，RDE)； 步驟三、工作空間坐標(biāo)系建立的S03階段:在機(jī)械臂(I)基座處建立機(jī)械臂坐標(biāo)系(ORXRYRZR);靶標(biāo)坐標(biāo)系(OEXEYEZE)在編碼特征點(diǎn)或靶標(biāo)其他位置上選?。荒┒藞?zhí)行器坐標(biāo)系(ODXDYDZD)建立在機(jī)械臂末端執(zhí)彳了器(3)的末端； 步驟四、機(jī)械臂到達(dá)預(yù)定位置的S04階段:通過上位機(jī)(6)輸入目標(biāo)空間坐標(biāo)參數(shù)，利用機(jī)械臂控制柜(4)控制機(jī)械臂(I)到達(dá)預(yù)定位置； 步驟五、靶標(biāo)實(shí)時(shí)成像測(cè)量的S05階段:測(cè)量像機(jī)(5)對(duì)安裝在機(jī)械臂(I)與機(jī)械臂末端執(zhí)行器(3)之間的多面體編碼點(diǎn)特征測(cè)量靶標(biāo)(2)進(jìn)行實(shí)時(shí)成像測(cè)量，然后將圖像數(shù)據(jù)傳遞到上位機(jī)(6);步驟六、獲取機(jī)械臂末端執(zhí)行器姿態(tài)的S06階段:上位機(jī)(6)對(duì)上述測(cè)量圖像經(jīng)過編碼特征點(diǎn)掃描、質(zhì)心定位、編碼特征點(diǎn)識(shí)別，后方交會(huì)求解像機(jī)外方位和相對(duì)方位，利用外極線約束外極角匹配方法進(jìn)行編碼特征點(diǎn)匹配，基于共線性約束的優(yōu)化平差求解編碼特征點(diǎn)空間三維坐標(biāo)；像機(jī)基站成像中利用空間存在多于3點(diǎn)的相互之間空間距離已知的特征點(diǎn)與成像點(diǎn)之間的關(guān)系，采用基于空間共角約束條件下的交會(huì)姿態(tài)估計(jì)算法和多外部方位裝置優(yōu)化確定像機(jī)基站的絕對(duì)外部方位(TCW，RCW)，即像機(jī)坐標(biāo)系(OCXCYCZC)在工件坐標(biāo)系(0WXWYWZW)下的位姿(TCW，RCff);通過坐標(biāo)逆變換獲得靶標(biāo)坐標(biāo)系在像機(jī)坐標(biāo)系(OCXCYCZC)下的位姿(TEC，REC)； 步驟七、獲取機(jī)械臂末端執(zhí)行器精確空間坐標(biāo)的S07階段:利用機(jī)械臂末端執(zhí)行器(3)與多面體編碼點(diǎn)特征測(cè)量靶標(biāo)(2)之間的剛體連接關(guān)系和預(yù)校準(zhǔn)并建立的末端執(zhí)行器數(shù)據(jù)庫，最終獲得機(jī)械臂末端執(zhí)行器(3)在工件坐標(biāo)系下的精確坐標(biāo)位置及姿態(tài)； 步驟八、補(bǔ)償機(jī)械臂末端執(zhí)行器位姿偏差的S08階段:通過位姿檢測(cè)補(bǔ)償系統(tǒng)計(jì)算程序位姿和測(cè)量位姿偏差值，由上位機(jī)(6)發(fā)送補(bǔ)償位移給機(jī)械臂控制柜(4)，從而精確控制機(jī)械臂(I)的動(dòng)作； 步驟九、判斷預(yù)定動(dòng)作是否完成的S09階段:判斷本次操作的預(yù)定動(dòng)作是否全部完成，如果判斷結(jié)果為“是”，則本操作流程結(jié)束，否則下一步返回S03階段的入口處，繼續(xù)下一步動(dòng)作。
【文檔編號(hào)】G01B11/00GK103895023SQ201410135361
【公開日】2014年7月2日 申請(qǐng)日期:2014年4月4日 優(yōu)先權(quán)日:2014年4月4日
【發(fā)明者】于之靖, 孫海龍 申請(qǐng)人:中國民航大學(xué)