[發(fā)明專利]基于視覺(jué)測(cè)量的非接觸式挖掘機(jī)工作裝置姿態(tài)測(cè)量方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201410079270.6
• 申請(qǐng)日: 2014-03-06
• 公開(kāi)（公告）號(hào): CN103900497A
• 公開(kāi)（公告）日: 2014-07-02
• 發(fā)明（設(shè)計(jì)）人: 王海波;鄒海龍;張如照
• 申請(qǐng)（專利權(quán)）人: 西南交通大學(xué)
• 主分類號(hào): G01B11/26
• 分類號(hào): G01B11/26;G01B11/00
• 代理公司: 成都博通專利事務(wù)所 51208
• 代理人: 陳樹(shù)明

1.一種基于視覺(jué)測(cè)量的非接觸式工作裝置姿態(tài)測(cè)量方法，包括以下步驟：

A、攝相機(jī)與特征片的布置：

將工業(yè)攝相機(jī)（4）安裝在挖掘機(jī)駕駛室機(jī)架（5）上，工業(yè)攝相機(jī)（4）視場(chǎng)從一側(cè)覆蓋挖掘機(jī)的工作裝置，工業(yè)攝相機(jī)（4）的USB接口與挖掘機(jī)的自動(dòng)控制系統(tǒng)相連；

同時(shí)，在挖掘機(jī)動(dòng)臂（1）、斗桿（2）和鏟斗（3）面向工業(yè)攝相機(jī)（4）的側(cè)面均粘貼兩個(gè)圓形特征片（6），特征片（6）的表面分割為四個(gè)90度扇形區(qū)，其中相對(duì)的兩個(gè)扇形區(qū)為黑色區(qū)（6a），另兩個(gè)相對(duì)的扇形區(qū)為白色區(qū)(6b)，使特征片（6）的中心點(diǎn)在圖像的鞍點(diǎn)檢測(cè)中能被檢測(cè)為鞍點(diǎn)；

動(dòng)臂（1）上的兩個(gè)特征片（6）的中心點(diǎn)連線與動(dòng)臂（1）上兩個(gè)桿件鉸點(diǎn)的連線平行；斗桿（2）上的兩個(gè)特征片（6）的中心點(diǎn)連線與斗桿（2）上兩個(gè)桿件鉸點(diǎn)的連線平行；鏟斗（3）上的兩個(gè)特征片（6）的中心點(diǎn)連線與鏟斗（3）的桿件鉸點(diǎn)和鏟尖的連線平行；

所述的動(dòng)臂（1）上的兩個(gè)特征片（6）的中心點(diǎn)距離為l₁，斗桿（2）上的兩個(gè)特征片（6）的中心點(diǎn)距離為l₂，鏟斗（3）上的兩個(gè)特征片（6）的中心點(diǎn)距離為l₃，且l₁≠l₂≠l₃,l₁≠l₃；

B、圖像獲取：當(dāng)挖掘機(jī)工作時(shí)，工業(yè)攝相機(jī)（4）每隔60～80ms攝取一幅數(shù)字圖像，并通過(guò)USB接口將數(shù)字圖像傳遞給自動(dòng)控制系統(tǒng)；

C、鞍點(diǎn)檢測(cè)：

初次檢測(cè)時(shí)，自動(dòng)控制系統(tǒng)通過(guò)鞍點(diǎn)搜索算法在整個(gè)數(shù)字圖像中檢測(cè)出鞍點(diǎn)i，得到鞍點(diǎn)i的圖像坐標(biāo)系坐標(biāo)P’_i(r_i,c_i)，其中，i為鞍點(diǎn)的序號(hào)、i=1,2,…n，r為鞍點(diǎn)的行坐標(biāo)，c為鞍點(diǎn)的列坐標(biāo)；

初次以后的檢測(cè)，自動(dòng)控制系統(tǒng)則通過(guò)鞍點(diǎn)搜索算法在前次檢測(cè)的步驟F確定的矩形檢測(cè)區(qū)域中檢測(cè)出鞍點(diǎn)i，得到鞍點(diǎn)i的圖像坐標(biāo)系坐標(biāo)P’_i(r_i,c_i)，其中，i為鞍點(diǎn)的序號(hào)、i=1,2,…n，r為鞍點(diǎn)的行坐標(biāo)，c為鞍點(diǎn)的列坐標(biāo)；

D、坐標(biāo)變換：將鞍點(diǎn)i的圖像坐標(biāo)系坐標(biāo)P’_i(r,c)轉(zhuǎn)換為以動(dòng)臂（1）、斗桿（2）和鏟斗（3）的被攝側(cè)面為x-y平面的世界坐標(biāo)系(WCS)的坐標(biāo)P_i(x_i?y_i,0)；

E、鞍點(diǎn)過(guò)濾與傾角測(cè)量：計(jì)算任意兩鞍點(diǎn)在世界坐標(biāo)系中的相互距離即鞍點(diǎn)距離a_jk，得到距離矩陣L：

L=0a12a13...a1n0a23...a2n0...a23.........0]]>

式中：a_jk表示第i=j個(gè)鞍點(diǎn)與第i=k點(diǎn)個(gè)鞍點(diǎn)之間的距離，L為對(duì)稱矩陣；

計(jì)算所有鞍點(diǎn)距離a_jk與動(dòng)臂（1）上兩個(gè)特征片（6）的鞍點(diǎn)距離l₁的差值的絕對(duì)值|a_jk-l₁|，若|a_j1,k1-l₁|的值為最小，則判定對(duì)應(yīng)的兩個(gè)鞍點(diǎn)j1和k1即為動(dòng)臂上的兩個(gè)特征片（6）的中心點(diǎn)；兩中心點(diǎn)連線的傾角為α₁：

a1=arctan(yj1-yk1xj1-xk1)]]>

計(jì)算所有鞍點(diǎn)距離a_jk與斗桿（2）上兩個(gè)特征片（6）的鞍點(diǎn)距離l₂的差值的絕對(duì)值|a_jk-l₂|，若|a_j2,k2-l₂|的值為最小，則判定對(duì)應(yīng)的兩個(gè)鞍點(diǎn)j2和k2即為動(dòng)臂上的兩個(gè)特征片（6）的中心點(diǎn)；兩中心點(diǎn)連線的傾角為α₂：

a2=arctan(yj2-yk2xj2-xk2)]]>

計(jì)算所有鞍點(diǎn)距離a_jk與鏟斗（3）上兩個(gè)特征片（6）的鞍點(diǎn)距離l3的差值的絕對(duì)值|a_jk-l₃|，若|a_j3,k3-l₃|的值為最小，則判定對(duì)應(yīng)的兩個(gè)鞍點(diǎn)j3和k3即為動(dòng)臂上的兩個(gè)特征片（6）的中心點(diǎn)；兩中心點(diǎn)連線的傾角為α₃：

a3=arctan(yj3-yk3xj3-xk3)]]>

F、縮小檢測(cè)區(qū)域：在當(dāng)前檢測(cè)到的六個(gè)特征片（6）的中心點(diǎn)世界坐標(biāo)系坐標(biāo)中，找出最左的中心點(diǎn)坐標(biāo)L(x_l,y_l,0)、最右的中心點(diǎn)坐標(biāo)R(x_r,y_r,0)、最上的中心點(diǎn)坐標(biāo)T(x_t,y_t,0)和最下的中心點(diǎn)坐標(biāo)B(x_b,y_b,0)，x₀為設(shè)定的所有特征片（6）在檢測(cè)時(shí)間間隔內(nèi)橫向運(yùn)動(dòng)的最大距離，y₀為所有特征片在檢測(cè)時(shí)間間隔內(nèi)縱向運(yùn)動(dòng)的最大距離；則下一次檢測(cè)的矩形區(qū)域的兩個(gè)對(duì)角點(diǎn)坐標(biāo)為：LB(x_l-x₀,y_l-y₀,0)、TR(x_r+x₀,y_r+y₀,0)，再通過(guò)坐標(biāo)反變換即可得到下一次矩形檢測(cè)區(qū)域的兩個(gè)對(duì)角點(diǎn)的圖像坐標(biāo)系坐標(biāo)，進(jìn)而確定出下一次檢測(cè)的圖像坐標(biāo)系中的矩形檢測(cè)區(qū)域；

G、重復(fù)B-F步驟，即可實(shí)時(shí)地動(dòng)態(tài)測(cè)量出挖掘機(jī)工作裝置的姿態(tài)。