操縱器關(guān)節(jié)極限處理算法
【專利摘要】分析具有在m個自由度的任務(wù)空間操作的n個關(guān)節(jié)(112)的自動機(jī)械設(shè)備(100)的所需移動命令(203)，以判斷它是否將導(dǎo)致關(guān)節(jié)角極限中的任何一個被違犯。在關(guān)節(jié)(112)中的非零數(shù)量L(241)個關(guān)節(jié)具有被違犯的角極限的情況下，根據(jù)下列公式，構(gòu)建修改的移動命令(254)：其中：qmod(253)是當(dāng)前沒有被限制的關(guān)節(jié)的(n-L)×1關(guān)節(jié)速度命令，qnew(254)是n×1新關(guān)節(jié)速度命令，Jmod(251)是m×(n-L)矩陣，是Jmod轉(zhuǎn)置矩陣，W(252)是包含加權(quán)因數(shù)的m×m矩陣，而xcmd(203)是末端執(zhí)行器(116)速度的所需移動命令(203)。通過確定自動機(jī)械設(shè)備(100)的m×n雅各布矩陣J(211)，然后使用雅各布矩陣J(211)來生成矩陣Jmod(251)，構(gòu)建Jmod(251)，其中，Jmod(251)包括J(211)中的除對應(yīng)于L個有效限制的關(guān)節(jié)的那些列之外的所有列。
【專利說明】操縱器關(guān)節(jié)極限處理算法
[0001]本發(fā)明一般涉及對自動機(jī)械關(guān)節(jié)的控制。更具體而言，實(shí)施例公開了用于處理在自動機(jī)械系統(tǒng)中實(shí)現(xiàn)關(guān)節(jié)極限的情況的方法和相關(guān)的系統(tǒng)。
[0002]自動機(jī)械系統(tǒng)變得越來越普遍，并在諸如制造和遙感之類的各種領(lǐng)域得到應(yīng)用。如圖1所示，自動機(jī)械系統(tǒng)10可以包括安裝到基座14上的自動機(jī)械臂12,自動機(jī)械臂12可以活動。臂12具有末端執(zhí)行器16和多個關(guān)節(jié)18，以根據(jù)需要由例如遙控操作員來移動和定位末端執(zhí)行16。每一個關(guān)節(jié)具有現(xiàn)有技術(shù)中已知的各自的馬達(dá)或致動器，以產(chǎn)生該關(guān)節(jié)18的角位置的變化。確定關(guān)節(jié)18中的每一個的角度和空間定位以實(shí)現(xiàn)末端執(zhí)行器16的所需定位和速度是本領(lǐng)域已知的問題。
[0003]自動機(jī)械系統(tǒng)，特別是由人類操作員控制的系統(tǒng)，諸如系統(tǒng)10，經(jīng)常遇到的一個問題是，出現(xiàn)末端執(zhí)行器16的所需速度將導(dǎo)致一個或多個關(guān)節(jié)18達(dá)到其極限的情況。結(jié)果，自動機(jī)械臂12將在物理上不能履行命令。在這樣的情況下，系統(tǒng)10的典型響應(yīng)是忽略該命令并且不執(zhí)行任何操作，而同時或許向操作員指出所需動作不能執(zhí)行。然而，這種對末端執(zhí)行器16移動的“全要或全不要”方法不能令用戶滿意。
[0004]因此，需要提供用于控制自動機(jī)械臂的移動的替代的方法以及相關(guān)的系統(tǒng)。
[0005]一方面，公開了用于控制自動機(jī)械設(shè)備中的多個關(guān)節(jié)的方法。關(guān)節(jié)中的至少一部分具有各自的關(guān)節(jié)移動極限?；陉P(guān)節(jié)的類型(例如，外卷的，棱柱的等等)，關(guān)節(jié)移動可以是旋轉(zhuǎn)的、線性的等等，如此，關(guān)節(jié)移動極限可以據(jù)此被表達(dá)(例如，對角位移的限制)。大多數(shù)自動機(jī)械操縱器使用旋轉(zhuǎn)關(guān)節(jié)，并且關(guān)節(jié)移動用關(guān)節(jié)角度來表示。為簡明起見，為說明本發(fā)明，將只討論旋轉(zhuǎn)關(guān)節(jié)，但是應(yīng)該理解，所描述的算法可適用于以任何組合帶有任何類型的關(guān)節(jié)的自動機(jī)械機(jī)制。
[0006]分析自動機(jī)械設(shè)備的所需移動命令，以判斷它是否將導(dǎo)致關(guān)節(jié)極限中的任何一個被違犯。然后，使用自動機(jī)械設(shè)備中的關(guān)節(jié)的當(dāng)前角度狀態(tài)和所需移動命令，來構(gòu)建自動機(jī)械設(shè)備的修改的移動命令。構(gòu)建修改的移動命令，以便避免違犯相應(yīng)的關(guān)節(jié)極限中的任何一個。修改的移動命令不與所需移動命令相同，并且當(dāng)被自動機(jī)械設(shè)備處理時，導(dǎo)致自動機(jī)械設(shè)備的移動。然后，使用修改的移動命令來代替所需移動命令，以導(dǎo)致自動機(jī)械設(shè)備的相應(yīng)的移動。
[0007]在各種實(shí)施例中，使用加權(quán)關(guān)節(jié)的相應(yīng)的移動的加權(quán)因數(shù)來生成修改的移動命令。在其中自動機(jī)械設(shè)備包括η個關(guān)節(jié)的特定優(yōu)選實(shí)施例中，自動機(jī)械在m維工作空間操作，其中，所需移動命令將導(dǎo)致η個關(guān)節(jié)中的L個關(guān)節(jié)違犯它們的相應(yīng)的關(guān)節(jié)極限，根據(jù)下列公式構(gòu)建自動機(jī)械設(shè)備的修改的關(guān)節(jié)移動命令:
【權(quán)利要求】
1.一種用于控制自動機(jī)械設(shè)備中的多個關(guān)節(jié)的方法，所述關(guān)節(jié)中的至少一部分具有相應(yīng)的關(guān)節(jié)移動極限，所述方法包括: 判斷所述自動機(jī)械設(shè)備的所需移動命令是否將導(dǎo)致所述自動機(jī)械設(shè)備的至少第一關(guān)節(jié)移動極限被違犯； 使用所述自動機(jī)械設(shè)備的當(dāng)前狀態(tài)和所述所需移動命令，為所述自動機(jī)械設(shè)備構(gòu)建修改的移動命令，所述修改的移動命令避免所述至少第一關(guān)節(jié)移動極限被違犯，其中，所述修改的移動命令不與所述所需移動命令相同，并且當(dāng)被所述自動機(jī)械設(shè)備處理時導(dǎo)致所述自動機(jī)械設(shè)備的移動；以及 使用所述修改的移動命令代替所述所需移動命令，以導(dǎo)致所述自動機(jī)械設(shè)備的相應(yīng)的移動。
2.如權(quán)利要求1所述的方法，其中，為所述自動機(jī)械設(shè)備構(gòu)建修改的移動命令進(jìn)一步包括使用加權(quán)因數(shù)來加權(quán)所述關(guān)節(jié)的相應(yīng)的移動。
3.如權(quán)利要求2所述的方法，其中，所述自動機(jī)械設(shè)備包括所述自動機(jī)械設(shè)備的末端執(zhí)行器的m維任務(wù)空間的η個關(guān)節(jié)，其中，所述所需移動命令將導(dǎo)致所述η個關(guān)節(jié)中的L個違犯它們的相應(yīng)的關(guān)節(jié)移動極限，并且其中，為所述自動機(jī)械設(shè)備構(gòu)建所述修改的關(guān)節(jié)移動命令包括求解下列公式:
4.如權(quán)利要求3所述的方法，其中，為所述自動機(jī)械設(shè)備構(gòu)建修改的關(guān)節(jié)移動命令進(jìn)一步包括求解下列公式:
5.如權(quán)利要求3所述的萬法,進(jìn)一步包括使用下列步驟來生成mX(n-L)矩陣Jmod: 使用所述自動機(jī)械設(shè)備的當(dāng)前狀態(tài)來確定所述自動機(jī)械設(shè)備的mXn雅各布矩陣J，所述自動機(jī)械設(shè)備的所述當(dāng)前狀態(tài)包括所述η個關(guān)節(jié)中的每一個的當(dāng)前位移；以及 使用所述雅各布矩陣J來生成所述矩陣Jnrod,其中，Jnrod包括J中的除對應(yīng)于所述L個有效限制的關(guān)節(jié)的那些列之外的所有列。
6.一種用于控制自動機(jī)械設(shè)備的系統(tǒng)，所述自動機(jī)械設(shè)備包括移動在m個自由度的任務(wù)空間中操作的末端執(zhí)行器的關(guān)節(jié)，所述自動機(jī)械設(shè)備由η個致動器驅(qū)動，所述系統(tǒng)與致動器驅(qū)動電路進(jìn)行通信以控制所述致動器，所述系統(tǒng)包括: 與所述致動器驅(qū)動電路進(jìn)行通信的至少一個中央處理單元CPU ;以及與所述至少一個CPU進(jìn)行通信的存儲器，所述存儲器包括能夠由所述至少一個CPU執(zhí)行以促使所述至少一個CPU執(zhí)行下列步驟的程序代碼:判斷所述自動機(jī)械設(shè)備的所需移動命令是否將導(dǎo)致η個被致動的關(guān)節(jié)中的至少一個違犯相應(yīng)的關(guān)節(jié)移動極限； 使用所述自動機(jī)械設(shè)備的當(dāng)前狀態(tài)和所述所需移動命令，為所述自動機(jī)械設(shè)備構(gòu)建修改的移動命令，所述修改的移動命令防止η個關(guān)節(jié)中的所述至少一個違犯所述相應(yīng)的關(guān)節(jié)移動極限，其中，所述修改的移動命令不與所述所需移動命令相同，并且如果被提交給所述致動器驅(qū)動電路，導(dǎo)致所述η個關(guān)節(jié)中的至少一部分的移動；以及 使用所述修改的移動命令代替所述所需移動命令，來控制所述致動器驅(qū)動電路以導(dǎo)致所述自動機(jī)械設(shè)備的相應(yīng)的移動。
7.如權(quán)利要求6所述的系統(tǒng)，其中，為所述自動機(jī)械設(shè)備構(gòu)建修改的移動命令進(jìn)一步包括使用加權(quán)因數(shù)來加權(quán)所述關(guān)節(jié)的相應(yīng)的移動。
8.如權(quán)利要求7所述的系統(tǒng)，其中，所述所需移動命令將導(dǎo)致所述η個關(guān)節(jié)中的L個違犯它們的相應(yīng)的關(guān)節(jié)移動極限，并且其中，為所述自動機(jī)械設(shè)備構(gòu)建修改的移動命令包括求解下列公式:
9.如權(quán)利要求8所述的系統(tǒng)，其中，為所述自動機(jī)械設(shè)備構(gòu)建修改的移動命令進(jìn)一步包括求解下列公式:
10.如權(quán)利要求8所述的系統(tǒng)，其中，所述程序代碼進(jìn)一步包括用于生成mX(n-L)矩陣Jmod的下列步驟: 使用所述自動機(jī)械設(shè)備的當(dāng)前狀態(tài)，來確定所述自動機(jī)械設(shè)備的mXn雅各布矩陣J，所述自動機(jī)械設(shè)備的所述當(dāng)前狀態(tài)包括所述η個關(guān)節(jié)中的每一個的當(dāng)前位移；以及 使用所述雅各布矩陣J來生成所述矩陣Jnrod,其中，Jnrod包括J中的除對應(yīng)于L個有效限制的關(guān)節(jié)的那些列之外的所有列。
【文檔編號】B25J9/16GK103459101SQ201280013932
【公開日】2013年12月18日 申請日期:2012年3月2日 優(yōu)先權(quán)日:2011年3月22日
【發(fā)明者】M·D·薩默, P·M·鮑切爾 申請人:哈里公司