[發(fā)明專利]基于光學(xué)動作捕捉的示教機器人數(shù)據(jù)采集器系統(tǒng)

有效

• 申請?zhí)枺?/td>: 201910066936.7
• 申請日: 2019-01-24
• 公開（公告）號: CN109848964B
• 公開（公告）日: 2020-10-09
• 發(fā)明（設(shè)計）人: 禹鑫燚;徐佗成;柏繼華;歐林林;陸文祥
• 申請（專利權(quán)）人: 浙江工業(yè)大學(xué)
• 主分類號: B25J9/00
• 分類號: B25J9/00;B25J9/16
• 代理公司: 杭州天正專利事務(wù)所有限公司 33201
• 代理人: 王兵;黃美娟

1.一種基于光學(xué)動作捕捉的示教機器人數(shù)據(jù)采集系統(tǒng)，其特征在于：數(shù)據(jù)處理模塊通過交換機通信模塊對標定模塊發(fā)送采集命令，監(jiān)控模塊向各個模塊分發(fā)控制命令并監(jiān)視整個系統(tǒng)，實現(xiàn)機器人運動控制數(shù)據(jù)的實時在線采集，各模塊的具體構(gòu)成是：

標定模塊，標定模塊包括光學(xué)動作捕捉系統(tǒng)、作業(yè)工具和示教機器人，標定模塊中光學(xué)動作捕捉系統(tǒng)通過電纜線與交換機通信模塊相連接，光學(xué)動作捕捉系統(tǒng)中攝像頭接收到通過交換機通信模塊中傳輸來的數(shù)據(jù)采集命令，在光學(xué)動作捕捉系統(tǒng)中開始對作業(yè)工具進行光學(xué)動作捕捉，同時通過交換機通信模塊傳輸采集到的作業(yè)工具的運動軌跡數(shù)據(jù)；由于作業(yè)工具是基于光學(xué)動作捕捉系統(tǒng)坐標系，而機器人是處于世界坐標系下，因此需要將機器人置于光學(xué)動作捕捉系統(tǒng)中進行標定后保存機器人在光學(xué)動作捕捉系統(tǒng)坐標系中的相對位置，使得光學(xué)動作捕捉系統(tǒng)中可識別出機器人并對其進行操作，坐標變換過程如下：

其中，{V}為光學(xué)動作捕捉系統(tǒng)坐標系，{mar}為桌面參考物坐標系，{UR}為機器人基座坐標系，{tool}為作業(yè)工具坐標系，為4×4為UR坐標系相對于V坐標系的齊次變換，為mar坐標系相對于V坐標系的齊次變換，為UR坐標系相對于mar坐標系的齊次變換，為tool坐標系相對于UR坐標系的齊次變換，為的逆變換，為tool坐標系相對于V坐標系的齊次變換；表示了一般的旋轉(zhuǎn)和平移，來定義一個坐標系，為3×3旋轉(zhuǎn)矩陣，是{UR}相對于{V}的表達；標量r_ij，其中i＝1，2，3，j＝1，2,3，表示UR中每個矢量在其參考系V中單位方向上投影的分量來表示；^AP_URORG為確定坐標系UR的原點的位置矢量；

其余齊次變換與類似，表示用坐標系V表達時的坐標系UR主軸方向的單位矢量；在ROS中利用tf工具監(jiān)聽到光學(xué)動作捕捉系統(tǒng)坐標系、機器人底座桌面上標記物的坐標系，而機器人基座坐標系亦可通過tf工具監(jiān)聽到，因此根據(jù)發(fā)布的標記物與機器人基座的位置關(guān)系，通過坐標變換可得到光學(xué)動作捕捉系統(tǒng)坐標系與機器人基座坐標系的相對位置關(guān)系，從而實現(xiàn)了機器人在光學(xué)動作捕捉系統(tǒng)坐標系的標定，標定完成后，標定模塊采集接收作業(yè)工具的運動軌跡數(shù)據(jù)信息并將采集到的信息通過交換機通信模塊發(fā)送到數(shù)據(jù)處理模塊；

數(shù)據(jù)處理模塊，數(shù)據(jù)處理模塊通過WiFi與交換機通信模塊相連，接收標定模塊中采集到的作業(yè)工具的運動軌跡數(shù)據(jù)和空間姿態(tài)數(shù)據(jù)信息，將采集到的位姿信息寫入文件中，在ROS系統(tǒng)中讀寫文件，發(fā)送給機器人末端，通過逆運動學(xué)求解，得到各個關(guān)節(jié)角度信息，機器人逆運動學(xué)求解過程如下：

機械臂的關(guān)節(jié)角度分別為基座為第一關(guān)節(jié)角θ₁，肩部為第二關(guān)節(jié)角θ₂，肘部為第三關(guān)節(jié)角θ₃，手腕1為第四關(guān)節(jié)角θ₄，手腕2為第五關(guān)節(jié)角θ₅，手腕3為第六關(guān)節(jié)角θ₆；具體的計算方法如下：

當機器人末端位姿的數(shù)值已知時，通過下列方程解出角度θ_i，i＝1，2，3，4，5，6；

其中，是末端坐標系6到固定坐標系0的變換矩陣，它的數(shù)值是已知的，n_x、n_y、n_z為末端坐標系6的每個矢量在參考坐標系固定坐標系0中x單位方向上的投影，o_x、o_y、o_z為末端坐標系6的每個矢量在參考坐標系固定坐標系0中的y單位方向上的投影，a_x、a_y、a_z為末端坐標系6的每個矢量在參考坐標系固定坐標系0中的z單位方向上的投影，根據(jù)式可以求出θ₁，可以表示為：

另外：

其中，其中c代表cos，s代表sin，θ_i：繞z_i坐標軸，從x_i-1旋轉(zhuǎn)到x_i的角度；d_i：沿z_i坐標軸，從x_i-1移動到x_i的角度；是末端坐標系6到固定坐標系1的變換矩陣，它的數(shù)值是已知的，¹n_x、¹n_y、¹n_z為末端坐標系6的每個矢量在參考坐標系固定坐標系1中x單位方向上的投影，¹o_x、¹o_y、¹o_z為末端坐標系6的每個矢量在參考坐標系固定坐標系1中的y單位方向上的投影，¹a_x、¹a_y、¹a_z為末端坐標系6的每個矢量在參考坐標系固定坐標系1中的z單位方向上的投影，對比式(1-6)和式(1-7)，兩個式子第2行第4列的值為常數(shù)，即可獲得求解θ₁需要的一元方程：

-p_xs₁+p_yc₁＝d₂+d₃+d₄ (1-8)

根據(jù)三角函數(shù)的萬能公式，可以求出θ₁：

θ₁＝atan2(p_y,p_x)-atan2(E,±F) (1-9)

E＝d₂+d₃+d₄；接著對比公式(1-6)和(1-7)，可以求解θ₅和θ₆；由矩陣的第二行的前三列的元素可以求解：

-n_xs₁+n_yc₁＝-s₅c₆ (1-10)

-o_xs₁+o_yc₁＝s₅c₆ (1-11)

-a_xs₁+a_yc₁＝-c₅ (1-12)

聯(lián)立(1-10)、(1-11)，消除變量，得：

求出θ₅：

θ₅＝atan2(s₅,a_xs₁-a_yc₁) (1-14)

當s₅＝0時，即θ₅＝0或θ₅＝π時，機構(gòu)會發(fā)生奇異性，這樣就不能求出θ₆的角度了；當s₅≠0，即θ₅≠0而且θ₅≠π時，可以求出θ₆：

對比公式(1-6)和(1-7)，根據(jù)矩陣第一行第三列的元素和第三行第三列元素，這兩列元素可以推導(dǎo)出以下的方程：

a_xc₁+a_ys₁＝-c₂₃₄s₅ (1-16)

a_z＝s₂₃₄s₅ (1-17)

當s₅＝0時，即θ₅＝0或θ₅＝π時，機構(gòu)會發(fā)生奇異性，這樣就不能求出θ₂+θ₃+θ₄的角度了；當s₅≠0，即θ₅≠0而且θ₅≠π時，可以求出θ₂+θ₃+θ₄：

對比公式(1-6)和(1-7)，由矩陣第一行第四列和第三行第四列這兩列的元素可以推導(dǎo)出以下方程：

p_xc₁+p_ys₁＝a₃c₂₃+a₂c₂-d₅s₂₃₄ (1-19)

p_z-d₁＝-a₃s₂₃-a₂s₂-d₅c₂₃₄ (1-20)

消除θ₂+θ₃，從而可以求解出θ₂的值：

解出θ₂之后，再次聯(lián)立(1-19)和(1-20)，可以求出θ₂+θ₃因此求出θ₃與θ₄；由以上的逆運動學(xué)的求解公式，就可以求出每個關(guān)節(jié)角的角度；

交換機通信模塊，交換機通信模塊包括以太網(wǎng)交換機、路由器和電纜線，以太網(wǎng)交換機通過電纜線接收數(shù)據(jù)處理模塊發(fā)送的采集命令，并將標定模塊采集到的運動軌跡數(shù)據(jù)及空間姿態(tài)數(shù)據(jù)信息發(fā)送到數(shù)據(jù)處理模塊，交換機通信模塊通過WiFi與數(shù)據(jù)處理模塊及標定模塊建立無線連接，由路由器開啟WiFi熱點，采用TCP通信協(xié)議模式建立無線連接，并通過WiFi透傳模式快速傳輸數(shù)據(jù)；交換機通信模塊為光學(xué)動作捕捉系統(tǒng)中的攝像頭提供電源及數(shù)據(jù)通信，并將采集完成的數(shù)據(jù)通過WiFi發(fā)送到數(shù)據(jù)處理模塊；

監(jiān)控模塊，監(jiān)控模塊通過WiFi經(jīng)由交換機通信模塊與機器人、光學(xué)動作捕捉系統(tǒng)組成局域網(wǎng)，負責向各個模塊分發(fā)控制命令，同時帶有監(jiān)視功能，將各模塊工作狀態(tài)顯示出來用于觀察整個系統(tǒng)是否順利運行，也有助于在系統(tǒng)運行出現(xiàn)問題時，能幫助用戶順利定位到出問題的模塊；

示教機器人模塊，示教機器人模塊通過WiFi接入數(shù)據(jù)處理模塊，接受在數(shù)據(jù)處理模塊中末端軌跡姿態(tài)轉(zhuǎn)換為各個關(guān)節(jié)角的角度值，使得機器人完成與作業(yè)工具動作相對應(yīng)的動作。