一種雙串聯(lián)機械臂式移動操作機裝置的碰撞檢測方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種碰撞檢測方法，尤其涉及一種雙串聯(lián)機械臂式移動操作機裝置的 碰撞檢測方法。
【背景技術(shù)】
[0002] 在專利[CN201510124392]中，提出了一種雙串聯(lián)機械臂式移動操作機裝置的自 動化定位方法，但在實際工作中，由于雙串聯(lián)機械臂式移動操作機裝置的串聯(lián)式機械臂處 在動態(tài)的、不確定的非結(jié)構(gòu)性環(huán)境中，因此碰撞現(xiàn)象可能經(jīng)常發(fā)生，所以如何判斷和避免串 聯(lián)式機械臂的碰撞對于雙串聯(lián)機械臂式移動操作機裝置的實際應(yīng)用非常重要，然而未見到 針對雙串聯(lián)機械臂式移動操作機裝置的系統(tǒng)化的碰撞檢測方法。

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

[0003] 基于以上目的，本發(fā)明提出一種雙串聯(lián)機械臂式移動操作機裝置的碰撞檢測方 法，具體方法如下： 首先，針對雙串聯(lián)機械臂式移動操作機裝置的工作情況進行碰撞檢測理論分析，利用 機器人運動學理論知識，建立雙串聯(lián)機械臂式移動操作機裝置的運動學坐標系以及用戶坐 標系，建立坐標變換矩陣，導(dǎo)出各個關(guān)節(jié)中心點的坐標表達式，求得任意兩連桿之間的最 小距離表達式，采用包絡(luò)半徑法對兩連桿之間的最小距離與兩連桿的包絡(luò)半徑之和進行比 較，得到碰撞檢測方程。
[0004] 接著，針對單機械臂作業(yè)的情況進行分析，利用Matlab工具，結(jié)合蒙特卡羅法獲 取單機械臂作業(yè)可能發(fā)生碰撞的情況。
[0005] 然后，針對雙機械臂同時工作的情況進行分析，利用Matlab工具，結(jié)合蒙特卡羅 法獲取雙機械臂同時作業(yè)可能發(fā)生碰撞的情況。
[0006] 再后，針對雙串聯(lián)機械臂式移動操作機裝置的兩個串聯(lián)式機械臂末端的目標位 姿，利用Matlab工具以及雙串聯(lián)機械臂式移動操作機裝置的解析逆解表達式獲取兩個串 聯(lián)式機械臂的各個關(guān)節(jié)變量，其中€為第1個運動自由度，實現(xiàn)機械臂的擺動；奪為第2個 自由度，實現(xiàn)機械臂的俯仰為第3個自由度，實現(xiàn)機械臂的回轉(zhuǎn)；_為第4個自由度，實 現(xiàn)滑架俯仰為第5個自由度，實現(xiàn)滑架擺動；為:為第6個自由度，實現(xiàn)滑架的移動。
[0007] 最后，單機械臂作業(yè)時，針對蒙特卡羅法檢測出來的單機械臂各連桿之間可能發(fā) 生的碰撞情況進行單臂碰撞檢測；雙機械臂同時作業(yè)時，針對蒙特卡羅法檢測出來的兩個 串聯(lián)式機械臂各連桿之間可能發(fā)生的碰撞情況進行雙機械臂碰撞檢測。
[0008] 本發(fā)明的碰撞檢測方法的有益效果是：針對雙串聯(lián)機械臂式移動操作機裝置提出 了單機械臂作業(yè)及雙機械臂同時作業(yè)時碰撞檢測的系統(tǒng)方法，得到了檢測連桿碰撞的檢測 方程，分析了單機械臂和雙機械臂可能發(fā)生的碰撞情況，只對可能發(fā)生碰撞的情況進行碰 撞檢測，縮短了檢測時間，為實現(xiàn)雙串聯(lián)機械臂式移動操作機裝置的實時碰撞檢測奠定了 基礎(chǔ)。
【附圖說明】
[0009] 圖1是本發(fā)明的雙串聯(lián)機械臂式移動操作機裝置的結(jié)構(gòu)示意圖； 圖2是本發(fā)明的雙串聯(lián)機械臂式移動操作機裝置的運動學坐標系和用戶坐標系； 圖3是本發(fā)明的碰撞檢測方法的流程圖。 具體實施例
[0010] 本發(fā)明方法適用于圖1所示的雙串聯(lián)機械臂式移動操作機裝置，該裝置由車身和 兩個串聯(lián)式機械臂組成，兩個串聯(lián)式機械臂安裝在車身的前部上面左右對稱布置，定位的 執(zhí)行部件為兩個串聯(lián)式機械臂，串聯(lián)式機械臂包括：滑塊1，末端操作器2,滑架3,推進器4， 整臂擺動油缸5,升降油缸6,機械臂7,俯仰油缸8和擺角油缸9。
[0011] 機械臂7的固定部分和車身鉸接，機械臂7的回轉(zhuǎn)部分和滑架3鉸接。升降油缸 6布置在機械臂7的下面后側(cè)，升降油缸6的后側(cè)和整臂擺動油缸5鉸接、升降油缸6的前 側(cè)和機械臂7的固定部分鉸接。俯仰油缸8布置在機械臂7的下面前側(cè)，俯仰油缸8的后 側(cè)和機械臂7的回轉(zhuǎn)部分鉸接，俯仰油缸8的前側(cè)和滑架3鉸接。擺角油缸9布置在機械 臂7的前側(cè)，擺角油缸9的后側(cè)和機械臂7的回轉(zhuǎn)部分鉸接，擺角油缸9的前側(cè)和滑架3鉸 接。滑架3布置在機械臂7的上部，滑架3的后側(cè)和機械臂7的回轉(zhuǎn)部分鉸接，滑架3的前 側(cè)和擺角油缸9的前側(cè)鉸接。推進器4布置在滑架3的上部。末端操作器2布置在推進器 4的上部和推進器4形成移動副。滑塊1布置滑架3上部。左側(cè)和右側(cè)的串聯(lián)式機械臂結(jié) 構(gòu)基本相同，兩個串聯(lián)式機械臂的區(qū)別在于：左側(cè)的串聯(lián)式機械臂（簡稱左臂）的整臂擺動 油缸（5)布置在機械臂左側(cè)，右側(cè)的串聯(lián)式機械臂(簡稱右臂）的整臂擺動油缸（5)布置在 機械臂右側(cè)。
[0012] 針對如圖1所示的雙串聯(lián)機械臂式移動操作機裝置，碰撞檢測的過程如下所示： 第一步，碰撞檢測方程求取 建立雙串聯(lián)機械臂式移動操作機裝置的運動學坐標系和用戶坐標系如圖2所示。考 慮到研究的雙串聯(lián)機械臂式移動操作機裝置為雙機械臂操作，且兩個機械結(jié)構(gòu)基本一致， 兩個串聯(lián)式機械臂的D-Η參數(shù)中的常量參數(shù)完全一致。以某實際裝置為例，根據(jù)建立的坐 標系確定D-Η參數(shù)如表1所示，分析方位初始角為，琴=:Θ氣烏=，爲=〇°，巧=， ：~ -90° 0
[0013] 表1串聯(lián)式機械臂的各連桿D-Η參數(shù)
用戶坐標系到串聯(lián)式機械臂的J變換矩陣，其中用戶坐標系到左臂的變換式中:巧<〇， 用戶坐標系到右臂的變換式中，<〇。
[0014] 利用圖2的運動學坐標系以及坐標變換矩陣4-4，導(dǎo)出每個連桿的兩端的關(guān)節(jié) 中心點坐標。
[0015] 設(shè)定用戶坐標系原點為：，則基坐標系原點為：
[0016] 第1個坐標系原點為：Q= (1)

將坐標系原點4對應(yīng)的點記為點4，將點讀與點1?.之間的連桿記作為4j+i?？紤] 連桿L+1與連桿的碰撞問題，將兩連桿上任意兩點之間的距離記作為，利用 的最小值來判斷兩連桿之間是否發(fā)生碰撞。連桿|_與連桿(^_+1所在直線上的點可 分別表不為：
通過對式（7)的分析，結(jié)合空間兩條直線之間的關(guān)系，將單機械臂運動關(guān)節(jié)的碰撞檢測 分兩種情況討論：連桿b+1和匕/+1所在的兩條直線的公垂線的垂足均在連桿上；連桿b+1 和所在的兩條直線的公垂線的垂足不都在連桿上。
[0017] (1)當連桿心+1和幻+1所在的兩條直線的公垂線的垂足均在連桿上時，說明 0 且的最小值則為兩直線的公垂線的大小。兩直線的公垂線的大小 ~(^V)的求法是將ω和^求出然后帶入公式（7)即可。
[0018] 下面計算求解:轉(zhuǎn)和分。將&^0平方，兩端再分別對·和欠求偏導(dǎo)并令其為零得：
解得：
若兩直線的公垂線的大小&^4 >25 (j為連桿的包絡(luò)半徑，此處取兩個連桿的包絡(luò) 半徑相等)，兩個連桿之間不發(fā)生碰撞，否則兩連桿之間發(fā)生碰撞。
[0019] (2)當連桿。+1和連桿所在的兩條直線的公垂線的垂足不都在連桿上時，這 時⑷,V)e(0,1)X(0,1)，兩直線距離函數(shù)'ιΧV)2在區(qū)域(〇, 1)X(〇，1)內(nèi)沒有極值點，極值點只 能在[αι]χ[0,ι]的邊界上達到，即在區(qū)間（cu)x(cu)內(nèi)，兩直線之間距離沒有最值，最值只能 在邊界上取得。那么，判斷兩連桿是否發(fā)生碰撞的條件（即兩連桿之間的最小距離% :::)要 考慮多種情況。
[0020] 對_和，分別討論，求取兩連桿之間的最小距離，最后得出&kv；)在區(qū)域 [0,1]χ[0,1]的邊界上的最小值為：
若> 25，兩個連桿不會發(fā)生碰撞，否則兩連桿發(fā)生碰撞。
[0021 ] 碰撞檢測方法的具體流程如圖3所示。
[0022] 第二步，單機械臂作業(yè)時可能發(fā)生的碰撞情況分析 單機械臂作業(yè)時，利用Matlab工具，結(jié)合蒙特卡羅法，將碰撞檢測方程編寫成Matlab程序，通過觀察串聯(lián)式機械臂結(jié)構(gòu)，相鄰連桿之間不可能發(fā)生碰撞，所以單機械臂作業(yè)時只 考慮不相鄰連桿之間的碰撞。在碰撞方程的Matlab程序中應(yīng)用循環(huán)語句，將單機械臂的所 有不相鄰的連桿之間的碰撞情況編寫在內(nèi)，隨機的給定2000個點，對所有不相鄰連桿之間 的碰撞情況進行檢測，輸出結(jié)果顯示具體串聯(lián)式機械臂的哪兩個連桿之間有無碰撞。具體 步驟如下： 1、 在各關(guān)節(jié)變量的取值范圍內(nèi)（見表1 )，對每個關(guān)節(jié)變量取一定的步長值，從而可得到 _(截=;'._:_:)組變量值的組合； 2、 求解串聯(lián)式機械臂的碰撞檢測方程，確定串聯(lián)式機械臂兩連桿間最小距離距離方程 (見式（8)與式（9)); 3、 將步驟1中生成的λ組變量值代入所求得的碰撞檢測方程程序，得到和個方程解的 值，將其對