一種破拆機(jī)器人手眼協(xié)調(diào)定位裝置及方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及機(jī)器人定位技術(shù)，具體是一種破拆機(jī)器人手眼協(xié)調(diào)定位裝置及方法。
【背景技術(shù)】
[0002] 在面臨地震、泥石流等災(zāi)害時(shí)，需要緊急進(jìn)行救援來(lái)盡量降低人員及財(cái)產(chǎn)損失，因 此提高震后的應(yīng)急救援能力極其重要，用于災(zāi)害現(xiàn)場(chǎng)的安全應(yīng)急救災(zāi)設(shè)備也是"十二五"時(shí) 期中國(guó)機(jī)械工業(yè)的發(fā)展重點(diǎn)之一。
[0003] 日本的東芝、三菱公司等相繼開(kāi)展了用于危險(xiǎn)現(xiàn)場(chǎng)搬運(yùn)作業(yè)的機(jī)器人，如東芝公 司研發(fā)的Smart-M機(jī)器人可以搬運(yùn)5kg左右的重物，三菱公司研制的Swan機(jī)器人具有地形 適應(yīng)能力，并可以搬運(yùn)IOkg左右的中等載荷。但是這些破拆機(jī)器人一般均采用遙控器人 工手工操作方式進(jìn)行破拆作業(yè)，而在現(xiàn)場(chǎng)復(fù)雜和危險(xiǎn)的環(huán)境下，對(duì)操作員人身安全帶來(lái)很 大威脅。針對(duì)文獻(xiàn)"破拆救援機(jī)器人關(guān)鍵技術(shù)"（劉慶運(yùn).《現(xiàn)代制造工程》，2009,30(7)， pp. 149~153.)中提出未來(lái)破拆機(jī)器人具備的功能有智能破拆作業(yè)，實(shí)時(shí)運(yùn)動(dòng)規(guī)劃，傳感 與目標(biāo)識(shí)別等關(guān)鍵技術(shù)，本發(fā)明提出的裝置及方法能解決傳統(tǒng)的破拆機(jī)器人存在的作業(yè)目 標(biāo)定位及機(jī)械臂自主運(yùn)動(dòng)功能，填補(bǔ)破拆機(jī)器人智能破拆作業(yè)功能應(yīng)用的空白。

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

[0004] 本發(fā)明的目的在于提供一種破拆機(jī)器人手眼協(xié)調(diào)定位裝置及方法，解決了破拆機(jī) 器人作業(yè)目標(biāo)定位問(wèn)題。
[0005] 為實(shí)現(xiàn)上述目的，本發(fā)明提供如下技術(shù)方案：
[0006] -種破拆機(jī)器人手眼協(xié)調(diào)定位裝置，由多關(guān)節(jié)液壓驅(qū)動(dòng)機(jī)械臂、激光測(cè)距傳感器、 CAN總線控制器、車載電腦構(gòu)成；所述激光測(cè)距傳感器安裝在多關(guān)節(jié)液壓驅(qū)動(dòng)機(jī)械臂的末 端執(zhí)行器上，所述激光測(cè)距傳感器通過(guò)CAN總線控制器與車載電腦實(shí)現(xiàn)信息交互。
[0007] 作為本發(fā)明進(jìn)一步的方案：所述激光測(cè)距傳感器采用的是SICK LMS151激光測(cè)距 傳感器。
[0008] 作為本發(fā)明進(jìn)一步的方案：所述CAN總線控制器采用的是PEAK-USB CAN總線控制 器。
[0009] 作為本發(fā)明進(jìn)一步的方案：所述車載電腦采用的是TREK-753車載電腦。
[0010] -種利用所述的破拆機(jī)器人手眼協(xié)調(diào)定位裝置進(jìn)行定位的方法，包括以下步驟：
[0011] 1)激光測(cè)距傳感器將采集的距離信息通過(guò)CAN總線發(fā)送到車載電腦；
[0012] 2)車載電腦對(duì)接收到的距離信息進(jìn)行處理后，再通過(guò)CAN總線發(fā)送液壓元件的速 度驅(qū)動(dòng)信號(hào)來(lái)驅(qū)使多關(guān)節(jié)液壓驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)到指定目標(biāo)位置，完成機(jī)械臂自動(dòng)定位和運(yùn) 動(dòng)控制。
[0013] 作為本發(fā)明進(jìn)一步的方案：所述步驟2)中，車載電腦通過(guò)調(diào)用機(jī)械臂自動(dòng)運(yùn)動(dòng)控 制算法對(duì)接收到的距離信息進(jìn)行處理，具體包括以下步驟：
[0014] 21)將末端執(zhí)行器上的激光測(cè)距傳感器激光紅點(diǎn)對(duì)準(zhǔn)欲破拆的作業(yè)點(diǎn)；
[0015] 22)將獲得的激光距離值通過(guò)齊次變換矩陣轉(zhuǎn)為末端執(zhí)行器下坐標(biāo)；
[0016] 23)通過(guò)連桿變換矩陣將末端執(zhí)行器下坐標(biāo)轉(zhuǎn)為基坐標(biāo)系下坐標(biāo)；
[0017] 24)根據(jù)逆運(yùn)動(dòng)學(xué)方程求出的各關(guān)節(jié)角角度；
[0018] 25)通過(guò)對(duì)液壓執(zhí)行部件發(fā)送速度信號(hào)驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)。
[0019] 作為本發(fā)明進(jìn)一步的方案：安裝在多關(guān)節(jié)液壓驅(qū)動(dòng)機(jī)械臂的末端執(zhí)行器上的激光 測(cè)距傳感器的激光發(fā)射方法與末端執(zhí)行器某一坐標(biāo)軸方向一致。
[0020] 與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是：
[0021 ] 1、開(kāi)發(fā)一種破拆機(jī)器人手眼目標(biāo)定位裝置，可以有效滿足機(jī)器人在惡劣環(huán)境下進(jìn) 行精確作業(yè)定位的需要。
[0022] 2、設(shè)計(jì)依附于手眼目標(biāo)定位裝置的破拆機(jī)器人液壓器件驅(qū)動(dòng)多關(guān)節(jié)機(jī)械臂自主 運(yùn)動(dòng)控制算法解決了現(xiàn)有遙控器手工操作的動(dòng)作遲緩及作業(yè)點(diǎn)找不準(zhǔn)問(wèn)題。
【附圖說(shuō)明】
[0023] 圖1是本發(fā)明定位裝置的結(jié)構(gòu)示意圖；
[0024] 圖2是實(shí)施例的示意圖；
[0025] 圖3為破拆機(jī)器人多關(guān)節(jié)機(jī)械臂連桿坐標(biāo)系布局圖；
[0026] 圖4是本發(fā)明定位方法的流程圖；
[0027] 圖5為機(jī)械臂自動(dòng)運(yùn)動(dòng)控制算法的流程圖；
[0028] 圖中：1-多關(guān)節(jié)液壓驅(qū)動(dòng)機(jī)械臂、2-激光測(cè)距傳感器、3-CAN總線控制器、4-車載 電腦。
【具體實(shí)施方式】
[0029] 下面將結(jié)合本發(fā)明實(shí)施例，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述， 顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例?；诒景l(fā)明中的 實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒(méi)有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他實(shí)施例，都 屬于本發(fā)明保護(hù)的范圍。
[0030] 請(qǐng)參閱圖1~2,本發(fā)明實(shí)施例中，一種破拆機(jī)器人手眼協(xié)調(diào)定位裝置，由多關(guān)節(jié) 液壓驅(qū)動(dòng)機(jī)械臂1、激光測(cè)距傳感器2、CAN總線控制器3、車載電腦4構(gòu)成；激光測(cè)距傳感 器2安裝在多關(guān)節(jié)液壓驅(qū)動(dòng)機(jī)械臂1的末端執(zhí)行器上，其激光發(fā)射方法與末端執(zhí)行器某一 坐標(biāo)軸方向一致，激光測(cè)距傳感器2將采集的距離信息通過(guò)CAN總線發(fā)送到車載電腦，由車 載電腦進(jìn)一步調(diào)用機(jī)械臂自主運(yùn)動(dòng)控制算法進(jìn)行下一步處理，最后由車載電腦4通過(guò)CAN 總線發(fā)送液壓元件的速度驅(qū)動(dòng)信號(hào)來(lái)驅(qū)使多關(guān)節(jié)液壓驅(qū)動(dòng)機(jī)械臂1運(yùn)動(dòng)到指定目標(biāo)位置， 完成機(jī)械臂自動(dòng)定位和運(yùn)動(dòng)控制。定位裝置的控制原理如下：通過(guò)破拆機(jī)器人自帶云臺(tái)攝 像機(jī)，觀察到現(xiàn)場(chǎng)的畫(huà)面，通過(guò)遠(yuǎn)端控制器控制末端執(zhí)行器對(duì)準(zhǔn)破拆目標(biāo)，激光測(cè)距傳感器 2獲得破拆目標(biāo)的距離并通過(guò)CAN總線傳回車載電腦4,車載電腦4將激光距離信息、機(jī)械 臂當(dāng)前姿態(tài)、液壓缸有效運(yùn)動(dòng)范圍等數(shù)據(jù)代入機(jī)械臂自主運(yùn)動(dòng)控制算法，驅(qū)動(dòng)機(jī)械臂進(jìn)行 運(yùn)動(dòng)。
[0031] 多關(guān)節(jié)液壓驅(qū)動(dòng)機(jī)械臂1采用的是5關(guān)節(jié)液壓驅(qū)動(dòng)機(jī)械臂，激光測(cè)距傳感器2采 用的是SICK LMS151激光測(cè)距傳感器，CAN總線控制器3采用的是PEAK-USB CAN總線控制 器，車載電腦4采用的是TREK-753車載電腦。
[0032] 請(qǐng)參閱圖4, 一種利用所述的破拆機(jī)器人手眼協(xié)調(diào)定位裝置進(jìn)行定位的方法，包括 以下步驟：
[0033] 1)激光測(cè)距傳感器（2)將采集的距離信息通過(guò)CAN總線發(fā)送到車載電腦（4);
[0034] 2)車載電腦（4)對(duì)接收到的距離信息進(jìn)行處理后，再通過(guò)CAN總線發(fā)送液壓元件 的速度驅(qū)動(dòng)信號(hào)來(lái)驅(qū)使多關(guān)節(jié)液壓驅(qū)動(dòng)機(jī)械臂（1)運(yùn)動(dòng)到指定目標(biāo)位置，完成機(jī)械臂自動(dòng) 定位和運(yùn)動(dòng)控制。
[0035] 上述步驟2)中，車載電腦（4)通過(guò)調(diào)用機(jī)械臂自動(dòng)運(yùn)動(dòng)控制算法對(duì)接收到的距離 信息進(jìn)行處理，具體包括以下步驟：
[0036] 21)將末端執(zhí)行器上的激光測(cè)距傳感器（2)激光紅點(diǎn)對(duì)準(zhǔn)欲破拆的作業(yè)點(diǎn)；
[0037] 22)將獲得的激光距離值通過(guò)齊次變換矩陣轉(zhuǎn)為末端執(zhí)行器下坐標(biāo)；
[0038] 23)通過(guò)連桿變換矩陣將末端執(zhí)行器下坐標(biāo)轉(zhuǎn)為基坐標(biāo)系下坐標(biāo)；
[0039] 24)根據(jù)逆運(yùn)動(dòng)學(xué)方程求出的各關(guān)節(jié)角角度；
[0040] 25)通過(guò)對(duì)液壓執(zhí)行部件發(fā)送速度信號(hào)驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)。
[0041] 請(qǐng)參閱圖3和5,機(jī)械臂自主運(yùn)動(dòng)控制算法，步驟為：
[0042] 算法步驟為：
[0043] 1、根據(jù)定位裝置測(cè)出的激光測(cè)距傳感器2在末端執(zhí)行器安裝位置之間的轉(zhuǎn)換矩 陣將作業(yè)目標(biāo)距離轉(zhuǎn)為末端執(zhí)行器坐標(biāo)系下三維坐標(biāo)。激光測(cè)距傳感器2安裝在末端執(zhí)行 器坐標(biāo)系的Y軸原點(diǎn)，激光器發(fā)射方向與Y軸正向一致，當(dāng)前激光測(cè)距傳感器2的讀數(shù)是d 米，則作業(yè)目標(biāo)在第五坐標(biāo)系（關(guān)節(jié)5坐標(biāo)系，即末端執(zhí)行器坐標(biāo)系）下的坐標(biāo)表示則為 [0, d，0]〇
[0044] 2、通過(guò)基坐標(biāo)系與末端執(zhí)行器坐標(biāo)系之間的齊次轉(zhuǎn)換矩陣計(jì)算獲取激光測(cè)距系 統(tǒng)下的目標(biāo)物體坐標(biāo)。末端執(zhí)行器坐標(biāo)系（關(guān)節(jié)5坐標(biāo)系）相對(duì)基坐標(biāo)系（關(guān)節(jié)1坐標(biāo)
系）的齊次變換矩陣為 其中rn= cos Θ 5*(c〇s Θ 4*(c〇s Θ ,cos Θ 2*c〇 9 s Θ 3-cos Θ lf 木sin θ 2木sin θ 3) -sin Θ 4木(cos Θ i-cos θ 2木sin Θ 3+cos Θ i-cos Θ 3>!<sin θ 2))_s in θ 5*(cos θ 4*(cos θ ,cos θ 2*sin θ 3+cos θ ,cos θ 3*sin θ 2)+sin θ 4*(cos θ ,cos θ 2>!<co s θ 3-cos θ 々in θ 2*sin θ 3));
[0045] r12=-cos θ 5* (cos θ 4* (cos θ ^cos θ 2*sin θ 3+cos θ ^cos θ 3*sin θ 2)+sin θ 4>!<(c os θ i-cos θ 2木cos θ 3_cos θ i-sin θ 2木sin θ 3))_sin θ 5木(cos θ 4>!<(cos θ i-cos θ 2木cos θ 3_co s θ i氺sin θ 2氺sin θ 3)_sin θ 4^(cos θ i^cos θ 2氺sin θ 3+cos θ i^cos θ 3氺sin θ 2));
[0046] r13= -sin θ 1;
[0047] r21= cos θ 5* (cos θ 4* (cos θ 2*cos θ 3*sin θ「sin θ 々in θ 2*sin θ 3)-sin θ 4* (c os θ 2氺sin θ θ 3+cos θ 3氺sin θ θ 2))_sin θ 5氺(cos θ 4^(cos θ 2氺sin θ θ 3+co s θ 3氺sin θ θ 2)+sin θ 4^(cos θ 2^cos θ 3氺sin θ i-sin θ θ 2氺sin θ 3));
[0048] r22=-cos θ 5* (cos θ 4* (cos θ 2*sin θ ^sin θ 3+cos θ 3*sin θ ,sin θ 2)+sin θ 4>!<(c os θ 2^cos θ 3氺sin θ i-sin θ θ 2氺sin θ 3))_sin θ 5氺(cos θ 4^(cos θ 2^cos θ 3氺si