一種適用于多約束條件下視覺信息導(dǎo)引的機(jī)械臂規(guī)劃方法
【專利摘要】本發(fā)明涉及一種適用于多約束條件下視覺信息導(dǎo)引的機(jī)械臂規(guī)劃方法，包括如下步驟：(1)判斷機(jī)械臂當(dāng)前所處的工作階段；(2)獲取捕獲目標(biāo)位置，并計(jì)算目標(biāo)速度；(3)判斷是否滿足捕獲條件，如果是，則完成抓捕操作，如果否，則執(zhí)行步驟(4)；(4)判斷手眼相機(jī)是否穩(wěn)定捕獲，如果是，則執(zhí)行捕獲段機(jī)械臂規(guī)劃指令，如果否，則執(zhí)行步驟(5)；(5)判斷機(jī)械臂是否進(jìn)入危險(xiǎn)區(qū)，如果是，則執(zhí)行搜索接近段機(jī)械臂規(guī)劃指令，如果否，則執(zhí)行快速接近段機(jī)械臂規(guī)劃指令。該方法采用雙目相機(jī)和手眼相機(jī)作為視覺傳感器，保證機(jī)械臂規(guī)劃輸入信息有效性，并加入視覺信息缺失情況下的機(jī)械臂運(yùn)動(dòng)預(yù)測(cè)方法，使得方案完整性好且適應(yīng)性強(qiáng)。
【專利說明】
一種適用于多約束條件下視覺信息導(dǎo)引的機(jī)械臂規(guī)劃方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種包含多約束條件、基于視覺信息導(dǎo)引的機(jī)械臂規(guī)劃方法，屬于空 間操作領(lǐng)域。
【背景技術(shù)】
[0002] 人類長(zhǎng)期從事太空活動(dòng)，完全失效的或由于任務(wù)結(jié)束被放棄的衛(wèi)星停留在太空成 為太空垃圾，空間碎片數(shù)量不斷增加，不僅危及在軌運(yùn)行航天器的安全，而且對(duì)有限的空間 軌道資源也構(gòu)成了嚴(yán)重的威脅。為了盡可能凈化軌道環(huán)境，各國(guó)正在研究以衛(wèi)星維修和太 空垃圾清除為目的的在軌服務(wù)技術(shù)。由于空間機(jī)械臂具有控制精度高、靈活性強(qiáng)以及任務(wù) 適應(yīng)性強(qiáng)等優(yōu)點(diǎn)，基于機(jī)械臂操作的主動(dòng)清除技術(shù)已成為清除廢棄衛(wèi)星和空間碎片的主要 手段。
[0003] 廢棄衛(wèi)星及空間碎片等被清除對(duì)象姿態(tài)并未受控，因此為解決面向運(yùn)動(dòng)目標(biāo)的捕 獲問題，需要進(jìn)行機(jī)械臂在線運(yùn)動(dòng)規(guī)劃，文獻(xiàn)（賈慶軒，陳鋼，孫漢旭，鄭雙奇.基于A*算法 的空間機(jī)械臂避障路徑規(guī)劃.機(jī)械工程學(xué)報(bào)，2010,13)公開了利用A*算法在空間機(jī)械臂的 自由工作空間進(jìn)行無碰撞路徑搜索，實(shí)現(xiàn)了空間機(jī)械臂的避障路徑規(guī)劃，在應(yīng)用時(shí)并未涉 及視覺信息輸入，未構(gòu)造機(jī)械臂與雙目相機(jī)視場(chǎng)間的約束條件。文獻(xiàn)（CN200910073472,空 間機(jī)器人視覺測(cè)量、路徑規(guī)劃、GNC -體化仿真系統(tǒng)）中提出了一種空間機(jī)器人跟蹤、接近、 捕獲目標(biāo)過程的自主路徑規(guī)劃方法，利用手眼相機(jī)的相對(duì)位置與姿態(tài)測(cè)量值實(shí)現(xiàn)規(guī)劃空間 機(jī)器人的運(yùn)動(dòng)，并未涉及機(jī)械臂規(guī)劃的各種約束條件，也未考慮視覺信息導(dǎo)引和雙目相機(jī)、 手眼相機(jī)切換的機(jī)械臂規(guī)劃分段策略，難以滿足多約束條件下視覺信息引導(dǎo)的機(jī)械臂在線 規(guī)劃需求。

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

[0004] 本發(fā)明的目的在于解決現(xiàn)有技術(shù)的不足，提供了一種適用于視場(chǎng)約束、碰撞約束 和運(yùn)動(dòng)約束等多約束條件下的視覺信息導(dǎo)引的機(jī)械臂規(guī)劃方法，該方法采用雙目相機(jī)和手 眼相機(jī)作為視覺傳感器，充分利用兩類相機(jī)的優(yōu)勢(shì)，提出了基于視覺信息導(dǎo)引的機(jī)械臂分 段運(yùn)動(dòng)及切換策略，保證機(jī)械臂規(guī)劃輸入信息的有效性，并考慮視覺信息缺失情況下的機(jī) 械臂運(yùn)動(dòng)預(yù)測(cè)方法，有效提升算法運(yùn)算效率又保證了機(jī)械臂操作的安全性。
[0005] 為了解決【背景技術(shù)】中存在的問題，本發(fā)明提供了一種適用于多約束條件下視覺信 息導(dǎo)引的機(jī)械臂規(guī)劃方法，包括如下步驟：
[0006] (1)計(jì)算機(jī)械臂末端位置，判斷機(jī)械臂當(dāng)前所處的工作階段，所述工作階段包括 快速接近段、搜索接近段和捕獲段；
[0007] (2)獲取捕獲目標(biāo)位置，并用該位置計(jì)算目標(biāo)速度；
[0008] (3)判斷是否滿足捕獲條件，如果是，則完成抓捕操作，如果否，則執(zhí)行步驟（4);
[0009] (4)判斷手眼相機(jī)是否穩(wěn)定捕獲，如果是，則執(zhí)行捕獲段機(jī)械臂規(guī)劃指令，如果否， 則執(zhí)行步驟（5);
[0010] (5)判斷機(jī)械臂是否進(jìn)入危險(xiǎn)區(qū)，如果符合危險(xiǎn)區(qū)條件，則執(zhí)行搜索接近段機(jī)械臂 規(guī)劃指令，如果不符合危險(xiǎn)區(qū)條件，則執(zhí)行快速接近段機(jī)械臂規(guī)劃指令。
[0011] 所述快速接近段機(jī)械臂規(guī)劃指令包括如下步驟：
[0012] (1)根據(jù)目標(biāo)位置和機(jī)械臂末端位置與姿態(tài)，計(jì)算相對(duì)位置偏差與指向偏差，從而 計(jì)算期望的末端速度和角速度，通過矩陣求逆計(jì)算雅可比矩陣得到六個(gè)關(guān)節(jié)角的期望角速 度；
[0013] ⑵根據(jù)指令關(guān)節(jié)角速度和角速度、角加速度約束，調(diào)整期望的關(guān)節(jié)角速度，計(jì)算 三次多項(xiàng)式系數(shù)。
[0014] 所述搜索接近段機(jī)械臂規(guī)劃指令包括如下步驟：
[0015] (1)初始化最優(yōu)代價(jià)函數(shù)值，隨機(jī)生成一組關(guān)節(jié)角，對(duì)隨機(jī)關(guān)節(jié)角可行性進(jìn)行判 斷，判斷是否到達(dá)最大搜索次數(shù)；
[0016] (2)根據(jù)指令關(guān)節(jié)角速度和角速度、角加速度約束，調(diào)整期望的關(guān)節(jié)角速度，計(jì)算 三次多項(xiàng)式系數(shù)。
[0017] 所述捕獲段機(jī)械臂規(guī)劃指令包括如下步驟：
[0018] (1)根據(jù)三維目標(biāo)位置和機(jī)械臂末端位置與姿態(tài)，計(jì)算相對(duì)位置偏差與指向偏差， 從而計(jì)算期望的末端速度和角速度，通過矩陣求逆計(jì)算雅可比矩陣得到六個(gè)關(guān)節(jié)角的期望 角速度；
[0019] (2)根據(jù)指令關(guān)節(jié)角速度和角速度、角加速度約束，調(diào)整期望的關(guān)節(jié)角速度，計(jì)算 三次多項(xiàng)式系數(shù)。
[0020] 所述獲取捕獲目標(biāo)位置通過視覺信息獲取捕獲目標(biāo)的位置，具體步驟為：
[0021] 若所處階段為快速接近段，則優(yōu)先使用雙目相機(jī)，如果雙目相機(jī)失效則使用手眼 相機(jī)；若所處階段為搜索接近段，優(yōu)先使用雙目相機(jī)，如果雙目相機(jī)失效則使用手眼相機(jī)； 若所處階段為捕獲段，則優(yōu)先使用手眼相機(jī)，如果手眼相機(jī)失效則使用雙目相機(jī)。
[0022] 進(jìn)一步的，當(dāng)雙目相機(jī)、手眼相機(jī)均失效時(shí)，采用預(yù)測(cè)算法將雙目相機(jī)與手眼相機(jī) 失效前保存的數(shù)據(jù)進(jìn)行線性外插。
[0023] 所述危險(xiǎn)區(qū)判斷包括如下步驟：
[0024] (1)判斷各關(guān)節(jié)角是否超過危險(xiǎn)邊界；
[0025] (2)如果未超出危險(xiǎn)邊界，繼續(xù)判斷機(jī)械臂是否與本體發(fā)生碰撞；
[0026] (3)如果未發(fā)生碰撞，繼續(xù)判斷機(jī)械臂是否遮擋雙目相機(jī)視場(chǎng)；
[0027] 進(jìn)一步的，所述危險(xiǎn)邊界為150°~180°，機(jī)械臂與本體的最大安全距離為 100mm~300mm，機(jī)械臂各關(guān)節(jié)距離雙目相機(jī)視場(chǎng)的最小距離為100mm~300mm。
[0028] 本發(fā)明與現(xiàn)有技術(shù)相比具有如下有益效果：
[0029] (1)采用分段式的視覺信息導(dǎo)引和切換策略，充分發(fā)揮雙目相機(jī)和手眼相機(jī)的優(yōu) 勢(shì)，并且考慮工程應(yīng)用中可能出現(xiàn)的視覺信息缺失情況，通過視覺信息融合處理，保障方法 可行性和有效性，既提升算法運(yùn)算效率又保證了操作安全性；
[0030] (2)在機(jī)械臂規(guī)劃算法中考慮機(jī)械臂與雙目相機(jī)視線間的約束，有效防止機(jī)械臂 運(yùn)動(dòng)過程中遮擋視場(chǎng)；加入關(guān)節(jié)角限位約束，有效避免機(jī)械臂運(yùn)動(dòng)超出關(guān)節(jié)運(yùn)動(dòng)范圍；對(duì) 碰撞約束進(jìn)行檢測(cè)，從而提升機(jī)械臂操作的安全性。
【附圖說明】
[0031] 圖1是基于視覺信息導(dǎo)引和切換的機(jī)械臂規(guī)劃分段策略示意圖；
[0032] 圖2是視覺信息缺失條件下的機(jī)械臂運(yùn)動(dòng)預(yù)測(cè)方法示意圖；
[0033] 圖3是快速接近段機(jī)械臂規(guī)劃算法流程圖；
[0034] 圖4是搜索接近段機(jī)械臂規(guī)劃算法流程圖；
[0035] 圖5是捕獲段機(jī)械臂規(guī)劃算法流程圖；
[0036] 圖6是蒙特卡洛打靶仿真試驗(yàn)結(jié)果統(tǒng)計(jì)圖。
【具體實(shí)施方式】
[0037] 下面結(jié)合附圖和【具體實(shí)施方式】對(duì)本發(fā)明的技術(shù)方案做進(jìn)一步詳細(xì)說明。顯然，所 描述的實(shí)施例僅僅是本發(fā)明的一部分實(shí)施例，而不是全部的實(shí)施例。基于本發(fā)明的實(shí)施例， 本領(lǐng)域技術(shù)人員在沒有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他實(shí)施例，都屬于本發(fā)明要 求保護(hù)的范圍。
[0038] 考慮空間運(yùn)動(dòng)目標(biāo)抓捕的快速性和安全性，將在線捕獲段分成三個(gè)階段：快速接 近段、搜索接近段和捕獲段，如圖1所示。其中，快速接近段和搜索接近段是指機(jī)械臂通過 雙目相機(jī)向目標(biāo)接近并使手眼相機(jī)捕獲目標(biāo)的過程；捕獲段是指手眼相機(jī)觀測(cè)到目標(biāo)之后 指引機(jī)械臂末端捕獲目標(biāo)的過程。
[0039] 視覺信息缺失情況下的機(jī)械臂運(yùn)動(dòng)預(yù)測(cè)方法，如圖2所示。捕獲段之前，優(yōu)先使用 雙目相機(jī)；捕獲段開始，優(yōu)先使用手眼相機(jī)；當(dāng)優(yōu)先使用的相機(jī)失效時(shí)，采用另一相機(jī)；當(dāng) 兩套相機(jī)均失效時(shí)，采用預(yù)測(cè)算法估計(jì)目標(biāo)位置及速度。
[0040] 針對(duì)雙目相機(jī)視場(chǎng)約束、平臺(tái)碰撞約束和關(guān)節(jié)最大轉(zhuǎn)角約束，即危險(xiǎn)區(qū)判斷，主要 采取如下措施：
[0041] (1)雙目相機(jī)視線約束
[0042] 定義目標(biāo)和雙目三點(diǎn)組成的平面為視線平面，機(jī)械臂根據(jù)需要在該平面的一側(cè)運(yùn) 動(dòng)，選取位于機(jī)械臂運(yùn)動(dòng)側(cè)且與視線平面距離為d的平面為危險(xiǎn)區(qū)邊界，兩平面之間的區(qū) 域?yàn)槲ｋU(xiǎn)區(qū)。根據(jù)機(jī)械臂的結(jié)構(gòu)，通過計(jì)算機(jī)械臂關(guān)節(jié)和末端距離視線平面的距離可以判 斷機(jī)械臂是否進(jìn)入危險(xiǎn)區(qū)。
[0043] (2)平臺(tái)碰撞約束
[0044] 對(duì)于平臺(tái)上的障礙物，可以選擇平臺(tái)上最高的部件，固定耳片作為基準(zhǔn)建立障礙 平面。同樣選擇一定的安全裕度確定危險(xiǎn)區(qū)邊界，危險(xiǎn)區(qū)邊界和障礙平面之間的空間即為 危險(xiǎn)區(qū)。根據(jù)機(jī)械臂的結(jié)構(gòu)，通過計(jì)算機(jī)械臂關(guān)節(jié)和機(jī)械臂末端與障礙平面的距離可以判 斷機(jī)械臂是否進(jìn)入危險(xiǎn)區(qū)。
[0045] (3)關(guān)節(jié)最大轉(zhuǎn)角約束
[0046] 考慮機(jī)械臂關(guān)節(jié)轉(zhuǎn)角限制，確定危險(xiǎn)轉(zhuǎn)角邊界，當(dāng)機(jī)械臂某一關(guān)節(jié)轉(zhuǎn)角達(dá)到該邊 界值時(shí)，即認(rèn)為進(jìn)入了危險(xiǎn)區(qū)。
[0047] 當(dāng)機(jī)械臂運(yùn)動(dòng)到危險(xiǎn)區(qū)內(nèi)后，采用捕獲段機(jī)械臂規(guī)劃指令考慮各種約束向目標(biāo)接 近，直至手眼相機(jī)能夠穩(wěn)定捕獲目標(biāo)。
[0048] 適用于多約束條件下視覺信息導(dǎo)引的機(jī)械臂規(guī)劃具體方法如下：
[0049] (1)通過機(jī)械臂運(yùn)動(dòng)學(xué)計(jì)算機(jī)械臂末端位置pe，
[0050] (2)判斷當(dāng)前所屬的工作階段是否滿足捕獲條件，如果符合，執(zhí)行步驟（7)，如果 不符合，執(zhí)行步驟（3);
[0051] (3)通過視覺信息使用策略計(jì)算目標(biāo)速度與位置具體步驟如下：
[0052] (a)若所處階段為快速接近段，則優(yōu)先使用雙目相機(jī)，進(jìn)入步驟（b)，如果雙目相 機(jī)失效則使用手眼相機(jī)，進(jìn)入步驟（c)，仍失效則進(jìn)入步驟（d);若所處階段為搜索接近段， 優(yōu)先使用雙目相機(jī)，進(jìn)入步驟（b)，如果雙目相機(jī)失效則使用手眼相機(jī)，進(jìn)入步驟（c)，仍失 效則進(jìn)入步驟（d);若所處階段為捕獲段，則優(yōu)先使用手眼相機(jī)，進(jìn)入步驟（c)，如果手眼相 機(jī)失效則使用雙目相機(jī)，進(jìn)入步驟（b)，仍失效則進(jìn)入步驟（d);
[0053] (b)利用雙目相機(jī)觀測(cè)值計(jì)算目標(biāo)位置rt_SV，并用該位置估算目標(biāo)速度vt，
將該速度值寫入目標(biāo)速度估算量Vt，并將最近目標(biāo)位置rt = rt_ SV加入序列，返回步驟（4);
[0054] (c)利用手眼相機(jī)觀測(cè)值計(jì)算目標(biāo)位置rt_HE，并將最近目標(biāo)位置計(jì)算 值rt = Fe (1:3, 1:3) ? rt_HE+Fe (1:3, 4)加入序列，并用該位置估算目標(biāo)速度vt，
將該速度值寫入目標(biāo)速度估算量Vt，返回步驟（4);
[0055] (d)采用預(yù)測(cè)算法將雙目相機(jī)與手眼相機(jī)失效前保存的數(shù)據(jù)進(jìn)行線性外插，返回 步驟（4);
[0056] (4)計(jì)算指向偏差角 aimangle = acos([0 ;0 ;1] ?rt/norm(rt))，根據(jù)機(jī)械臂末端 與目標(biāo)相對(duì)距離dret = rt-pe(rt為目標(biāo)位置）和指向偏差角aimangle判斷手眼相機(jī)是 否穩(wěn)定捕獲，若|(1代1：|〈1'1：_1：11代811〇1(1，且3；[1]^1^16〈3；[111_1：11代81101(1，則進(jìn)入步驟（5);否則 進(jìn)入步驟（6);
[0057] (5)判斷是否進(jìn)入危險(xiǎn)區(qū)，具體步驟如下：
[0058] (e)判斷各關(guān)節(jié)角是否達(dá)到危險(xiǎn)邊界safeangle，若if | JiRsafeangle(設(shè)定 safeangle=150° )，則認(rèn)為未達(dá)到危險(xiǎn)邊界，進(jìn)入步驟（f);若if|j」彡safeangle，則認(rèn) 為達(dá)到危險(xiǎn)邊界，執(zhí)行搜索接近段規(guī)劃算法；
[0059] (f)判斷機(jī)械臂是否與本體發(fā)生碰撞：若if | Pi (x) |〈safedist (設(shè)定safedist = 150mm)，則認(rèn)為未發(fā)生碰撞，則進(jìn)入步驟（g);若if | Pi (x) |彡safedist，則認(rèn)為發(fā)生碰撞， 執(zhí)行搜索接近段規(guī)劃算法；
[0060] (g)判斷機(jī)械臂是否遮擋雙目相機(jī)視場(chǎng)：
[0061] 首先計(jì)算
，（i = 3, 4, 5, 6, e)，若 dXsafedist (設(shè)定safedist = 150mm)，則認(rèn)為未發(fā)生遮擋，執(zhí)行快速接近段規(guī)劃算法；否 則認(rèn)為發(fā)生遮擋，執(zhí)行搜索接近段規(guī)劃算法；
[0062] (6)將當(dāng)前時(shí)間代入三次多項(xiàng)式，完成一次機(jī)械臂規(guī)劃指令組求解并發(fā)送給機(jī)械 臂，執(zhí)行步驟（1)-步驟（2);
[0063] (7)完成抓捕操作，流程結(jié)束。
[0064] 如圖3所示，快速接近段規(guī)劃算法具體步驟如下：
[0065] (16)、根據(jù)三維目標(biāo)位置rt和機(jī)械臂末端位置pe，計(jì)算相對(duì)位置偏差DeltaR : A R = rt-pe-Fe (1:3, l:3)>!<[CapPosX ；CapPosY ；CapPosZ]T；
[0066] (17)、根據(jù)機(jī)械臂末端位置與姿態(tài)矩陣Fe和相對(duì)位置偏差DeltaR，計(jì)算指向偏差 DeltaPhi
ved^\ 「Ai?
[0067] (18)、計(jì)算期望的末端速度和角速度：=^2 其中A R = *- -* L. _J
[DeltaRX, DeltaRY, DeltaRZ]1；
[0068] (19)、通過矩陣求逆計(jì)算雅可比矩陣Jm的逆Jmlnv ;
[0069] (20)、根據(jù)ved，wed和Jmlnv，計(jì)算六個(gè)關(guān)節(jié)角的期望角速度：
[0071] (21)、根據(jù)指令關(guān)節(jié)角速度DCJ1~DCJ6和角速度、角加速度約束，調(diào)整期望的關(guān) 節(jié)角速度DCJ1~DCJ6 ;
[0072] (22)、根據(jù)期望的關(guān)節(jié)角速度DCJ1~DCJ6,計(jì)算指令關(guān)節(jié)角CJ1~CJ6 ;
[0073] (23)、根據(jù)上一時(shí)刻存儲(chǔ)的指令角速度DJ1~DJ6和指令關(guān)節(jié)角CJ1~CJ6、指令 角速度DCJ1-DCJ6,計(jì)算三次多項(xiàng)式系數(shù)ai3~aiO, i = 1~6,返回步驟（6);
[0074] 如圖4所示，搜索接近段規(guī)劃算法具體步驟如下：
[0075] (24)、最優(yōu)代價(jià)函數(shù)值置為OxFFFFFFFF，并將最優(yōu)關(guān)節(jié)角BJ1~BJ6置為當(dāng)前關(guān)節(jié) 角J1~J6 ;
[0076] (25)、隨機(jī)生成一組關(guān)節(jié)角RJ1~RJ6 ;
[0077] (26)、根據(jù)當(dāng)前關(guān)節(jié)角J1~J6和生成的關(guān)節(jié)角RJ1~RJ6,計(jì)算期望的關(guān)節(jié)角速 度 DCJ1 ~DCJ6 ;
[0078] (27)、根據(jù)最大角速度和角加速度約束調(diào)整期望關(guān)節(jié)角速度DCJ1~DCJ6 ;
[0079] (28)、根據(jù)關(guān)節(jié)角速度DCJ1~DCJ6,重新計(jì)算隨機(jī)關(guān)節(jié)角RJ1~RJ6 ;
[0080] (29)、按照步驟（5)的方法判斷各關(guān)節(jié)角是否可行；
[0081] (30)、若可行，則計(jì)算代價(jià)函數(shù)值，進(jìn)入步驟（31);若不可行，進(jìn)入步驟（32);
[0082] (31)、比較代價(jià)函數(shù)值TempVal是否小于目前最優(yōu)代價(jià)函數(shù)值BestVal，若是，則 將最優(yōu)代價(jià)函數(shù)值置為當(dāng)前代價(jià)函數(shù)值，并將最優(yōu)關(guān)節(jié)角BJ1~BJ6置為當(dāng)前隨機(jī)關(guān)節(jié)角 RJ1~RJ6 ;若否，進(jìn)入步驟（32);
[0083] (32)、判斷是否到達(dá)最大搜索次數(shù)SearchNum(可根據(jù)硬件處理速度在30~300 內(nèi)選擇），若到達(dá)，則進(jìn)入步驟（33);若未到達(dá)，則返回至步驟（25);
[0084] (33)、利用最優(yōu)關(guān)節(jié)角BJ1~BJ6和當(dāng)前關(guān)節(jié)角J1~J6計(jì)算各關(guān)節(jié)角速度 DCJ1~DCJ6,并根據(jù)最大角速度和角加速度調(diào)整期望關(guān)節(jié)角速度DCJ1~DCJ6 ;
[0085] (34)、根據(jù)期望的關(guān)節(jié)角速度DCJ1~DCJ6,計(jì)算指令關(guān)節(jié)角CJ1~CJ6 ;
[0086] (35)、根據(jù)上一時(shí)刻存儲(chǔ)的指令角速度DJ1~DJ6和指令關(guān)節(jié)角CJ1~CJ6、指令 角速度DCJ1-DCJ6,計(jì)算三次多項(xiàng)式系數(shù)ai3~aiO, i = 1~6,返回步驟（6);
[0087] 如圖5所示，捕獲段規(guī)劃算法具體步驟如下：
[0088] (36)、根據(jù)三維目標(biāo)位置rt和機(jī)械臂末端位置pe，計(jì)算相對(duì)位置偏差DeltaR : A R = rt-pe-Fe (1:3, l:3)>!<[CapPosX ；CapPosY ；CapPosZ]T；
[0089] (37)、根據(jù)機(jī)械臂末端位置與姿態(tài)矩陣Fe和相對(duì)位置偏差DeltaR，計(jì)算指向偏 差：DeltaPhi
ved~\ . f
[0090] (38)、計(jì)算期望的末端速度和角速度：D & 其中A R = [DeltaRX, DeltaRY, DeltaRZ]1；
[0091] (39)、通過矩陣求逆計(jì)算雅可比矩陣Jm的逆Jmlnv ;
[0092] (40)、根據(jù)ved，wed和Jmlnv，計(jì)算六個(gè)關(guān)節(jié)角的期望角速度：
[0093] :細(xì)―、ed
[wed\ ^
[0094] (41)、根據(jù)指令關(guān)節(jié)角速度DCJ1~DCJ6和角速度、角加速度約束，調(diào)整期望的關(guān) 節(jié)角速度DCJ1~DCJ6 ;
[0095] (42)、根據(jù)上一時(shí)刻存儲(chǔ)的指令角速度DJ1~DJ6和指令關(guān)節(jié)角CJ1~CJ6、指令 角速度DCJ1-DCJ6,計(jì)算三次多項(xiàng)式系數(shù)ai3~aiO, i = 1~6,返回步驟（6)。
[0096] 如圖6所示為蒙特卡洛打靶仿真試驗(yàn)結(jié)果統(tǒng)計(jì)圖。為驗(yàn)證本文提出的機(jī)械臂規(guī)劃 方法的有效性，搭建了抓捕可靠性仿真系統(tǒng)，綜合考慮機(jī)械臂控制誤差、視覺探測(cè)誤差以及 視覺信息延遲，采用蒙特卡洛打靶法，進(jìn)行有效性及可靠性驗(yàn)證。運(yùn)行2000次仿真，經(jīng)統(tǒng) 計(jì)，抓捕成功率為99. 95%，驗(yàn)證了本方法的可行性和有效性。
[0097] 對(duì)所公開的實(shí)施例的上述說明，使本領(lǐng)域?qū)I(yè)技術(shù)人員能夠?qū)崿F(xiàn)或使用本發(fā)明。 對(duì)這些實(shí)施例的多種修改對(duì)本領(lǐng)域的技術(shù)人員來說是顯而易見的，本文中所定義的一般原 理可以在不脫離本發(fā)明范圍的情況下，在其他實(shí)施例中實(shí)現(xiàn)。因此，本發(fā)明將不會(huì)被限制于 本文所示的這些實(shí)施例，而是要符合與本文所公開的原理和新穎特點(diǎn)相一致的最寬范圍。
【主權(quán)項(xiàng)】
1. 一種適用于多約束條件下視覺信息導(dǎo)引的機(jī)械臂規(guī)劃方法，其特征在于，包括如下 步驟： (1) 計(jì)算機(jī)械臂末端位置，判斷機(jī)械臂當(dāng)前所處的工作階段，所述工作階段包括快速 接近段、搜索接近段和捕獲段； (2) 獲取捕獲目標(biāo)位置，并用該位置計(jì)算目標(biāo)速度； (3) 判斷是否滿足捕獲條件，如果是，則完成抓捕操作，如果否，則執(zhí)行步驟（4); (4) 判斷手眼相機(jī)是否穩(wěn)定捕獲，如果是，則執(zhí)行捕獲段機(jī)械臂規(guī)劃指令，如果否，則執(zhí) 行步驟（5); (5) 判斷機(jī)械臂是否進(jìn)入危險(xiǎn)區(qū)，如果符合危險(xiǎn)區(qū)條件，則執(zhí)行搜索接近段機(jī)械臂規(guī)劃 指令，如果不符合危險(xiǎn)區(qū)條件，則執(zhí)行快速接近段機(jī)械臂規(guī)劃指令。2. 根據(jù)權(quán)利要求1所述的一種適用于多約束條件下視覺信息導(dǎo)引的機(jī)械臂規(guī)劃方法， 其特征在于，所述快速接近段機(jī)械臂規(guī)劃指令包括如下步驟： (1) 根據(jù)目標(biāo)位置和機(jī)械臂末端位置與姿態(tài)，計(jì)算相對(duì)位置偏差與指向偏差，從而計(jì)算 期望的末端速度和角速度，通過矩陣求逆計(jì)算雅可比矩陣得到關(guān)節(jié)角的期望角速度； (2) 根據(jù)指令關(guān)節(jié)角速度和角速度、角加速度約束，調(diào)整期望的關(guān)節(jié)角速度，計(jì)算三次 多項(xiàng)式系數(shù)。3. 根據(jù)權(quán)利要求1所述的一種適用于多約束條件下視覺信息導(dǎo)引的機(jī)械臂規(guī)劃方法， 其特征在于，所述搜索接近段機(jī)械臂規(guī)劃指令包括如下步驟： (1) 初始化最優(yōu)代價(jià)函數(shù)值，隨機(jī)生成一組關(guān)節(jié)角，對(duì)隨機(jī)關(guān)節(jié)角可行性進(jìn)行判斷，判 斷是否到達(dá)最大搜索次數(shù)； (2) 根據(jù)指令關(guān)節(jié)角速度和角速度、角加速度約束，調(diào)整期望的關(guān)節(jié)角速度，計(jì)算三次 多項(xiàng)式系數(shù)。4. 根據(jù)權(quán)利要求1所述的一種適用于多約束條件下視覺信息導(dǎo)引的機(jī)械臂規(guī)劃方法， 其特征在于，所述捕獲段機(jī)械臂規(guī)劃指令包括如下步驟： (1) 根據(jù)三維目標(biāo)位置和機(jī)械臂末端位置與姿態(tài)，計(jì)算相對(duì)位置偏差，從而計(jì)算期望的 末端速度和角速度，通過矩陣求逆計(jì)算雅可比矩陣得到關(guān)節(jié)角的期望角速度； (2) 根據(jù)指令關(guān)節(jié)角速度和角速度、角加速度約束，調(diào)整期望的關(guān)節(jié)角速度，計(jì)算三次 多項(xiàng)式系數(shù)。5. 根據(jù)權(quán)利要求1所述的一種適用于多約束條件下視覺信息導(dǎo)引的機(jī)械臂規(guī)劃方法， 其特征在于，所述步驟（2)通過視覺信息獲取捕獲目標(biāo)的位置，具體步驟為： 若所處階段為快速接近段，則優(yōu)先使用雙目相機(jī)，如果雙目相機(jī)失效則使用手眼相 機(jī)；若所處階段為搜索接近段，優(yōu)先使用雙目相機(jī)，如果雙目相機(jī)失效則使用手眼相機(jī)； 若所處階段為捕獲段，則優(yōu)先使用手眼相機(jī)，如果手眼相機(jī)失效則使用雙目相機(jī)。6. 根據(jù)權(quán)利要求5所述的一種適用于多約束條件下視覺信息導(dǎo)引的機(jī)械臂規(guī)劃方法， 其特征在于，當(dāng)雙目相機(jī)、手眼相機(jī)均失效時(shí)，采用預(yù)測(cè)算法將雙目相機(jī)與手眼相機(jī)失效前 保存的數(shù)據(jù)進(jìn)行線性外插。7. 根據(jù)權(quán)利要求1所述的一種適用于多約束條件下視覺信息導(dǎo)引的機(jī)械臂規(guī)劃方法， 其特征在于，所述危險(xiǎn)區(qū)判斷包括如下步驟： (1)判斷各關(guān)節(jié)角是否超過危險(xiǎn)邊界； (2) 如果未超出危險(xiǎn)邊界，繼續(xù)判斷機(jī)械臂是否與本體發(fā)生碰撞； (3) 如果未發(fā)生碰撞，繼續(xù)判斷機(jī)械臂是否遮擋雙目相機(jī)視場(chǎng)。8.根據(jù)權(quán)利要求7所述的一種適用于多約束條件下視覺信息導(dǎo)引的機(jī)械臂規(guī)劃方法， 其特征在于，所述危險(xiǎn)邊界為150°~180°，機(jī)械臂與本體的最大安全距離為100mm~ 300mm，機(jī)械臂各關(guān)節(jié)距離雙目相機(jī)視場(chǎng)的最小距離為100mm~300mm。
【文檔編號(hào)】B25J13/08GK106003104SQ201510387748
【公開日】2016年10月12日
【申請(qǐng)日】2015年7月3日
【發(fā)明人】焉寧, 張烽, 李揚(yáng), 申麟, 張旭輝, 高朝輝, 唐慶博, 陳蓉, 唐超
【申請(qǐng)人】中國(guó)運(yùn)載火箭技術(shù)研究院