一種基于投影幾何法的單關(guān)節(jié)故障機(jī)械臂模型重構(gòu)方法
【專利摘要】本發(fā)明公開(kāi)了一種基于投影幾何法的單關(guān)節(jié)故障機(jī)械臂模型重構(gòu)方法。其核心包括：基于旋量坐標(biāo)表征機(jī)械臂的運(yùn)動(dòng)學(xué)模型，由于不同故障關(guān)節(jié)在基礎(chǔ)坐標(biāo)系中的軸向不同，其故障角度影響各關(guān)節(jié)旋量坐標(biāo)的不同參數(shù)，因此將故障關(guān)節(jié)按其軸向進(jìn)行分類，針對(duì)不同類關(guān)節(jié)，采用投影法在其軸線的公垂面構(gòu)造投影平面，獲得運(yùn)動(dòng)學(xué)模型平面投影圖；根據(jù)各故障關(guān)節(jié)的平面投影圖，通過(guò)幾何分析，建立故障角度、連桿長(zhǎng)度與各關(guān)節(jié)旋量坐標(biāo)的關(guān)系式，完成單關(guān)節(jié)故障機(jī)械臂模型重構(gòu)。本發(fā)明解決了機(jī)械臂單關(guān)節(jié)故障后，原有運(yùn)動(dòng)學(xué)模型失效，無(wú)法進(jìn)行運(yùn)動(dòng)控制的問(wèn)題，同時(shí)其以旋量理論作為基礎(chǔ)，相比D?H參數(shù)方法，具有效率高、通用性強(qiáng)，適用于不同構(gòu)型機(jī)械臂的特點(diǎn)。
【專利說(shuō)明】
一種基于投影幾何法的單關(guān)節(jié)故障機(jī)械臂模型重構(gòu)方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種基于投影幾何法的單關(guān)節(jié)故障機(jī)械臂模型重構(gòu)方法，屬于機(jī)械臂 容錯(cuò)技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 由于太空環(huán)境具有超真空、高溫差、強(qiáng)輻射的特點(diǎn)，綜合考慮空間機(jī)械臂工作環(huán)境 的惡劣性以及關(guān)節(jié)內(nèi)部的結(jié)構(gòu)復(fù)雜性，在其長(zhǎng)期服役過(guò)程中極有可能出現(xiàn)關(guān)節(jié)失效現(xiàn)象。 空間機(jī)械臂一旦失效，通常無(wú)法進(jìn)行及時(shí)修復(fù)，這將導(dǎo)致空間任務(wù)的推遲甚至延誤，從而影 響到整個(gè)航天計(jì)劃的實(shí)施。目前，針對(duì)空間機(jī)械臂在軌操作過(guò)程中發(fā)生單一關(guān)節(jié)突發(fā)失效 的情形，鎖定故障關(guān)節(jié)是一種簡(jiǎn)單而有效的處理方法。關(guān)節(jié)鎖定后由于運(yùn)動(dòng)模型的損壞，機(jī) 械臂無(wú)法按照原規(guī)劃路徑完成在軌操作任務(wù)，從而導(dǎo)致任務(wù)的失敗。本發(fā)明為一種基于投 影幾何法的單關(guān)節(jié)故障機(jī)械臂模型重構(gòu)方法，能夠滿足在機(jī)械臂單關(guān)節(jié)損壞狀態(tài)下，最大 限度的完成在軌任務(wù)的需求，對(duì)于未來(lái)進(jìn)行空間探索，特別是空間機(jī)械臂在軌應(yīng)用具有十 分重要的理論意義和現(xiàn)實(shí)意義。
[0003] 目前，針對(duì)單關(guān)節(jié)故障的機(jī)械臂模型重構(gòu)方法研究成果較少，主流的研究成果為 基于D-Η參數(shù)方法的SSRMS型機(jī)械臂的運(yùn)動(dòng)學(xué)模型重構(gòu)，解決了特殊構(gòu)型機(jī)械臂任一關(guān)節(jié)鎖 定時(shí)的模型重構(gòu)問(wèn)題，但其D-Η坐標(biāo)系的重建過(guò)程復(fù)雜，針對(duì)每一關(guān)節(jié)失效情況的重建方法 均不相同，因此計(jì)算量較大，同時(shí)針對(duì)不同構(gòu)型機(jī)械臂的解決方法沒(méi)有通用的表達(dá)式，因此 通用性低。為了解決上述問(wèn)題，本發(fā)明以旋量理論為基礎(chǔ)，提供了一種基于投影幾何法的單 關(guān)節(jié)故障機(jī)械臂模型重構(gòu)方法，該方法具有效率高、通用性強(qiáng)、適用于不同構(gòu)型機(jī)械臂的優(yōu) 點(diǎn)。
【發(fā)明內(nèi)容】
[0004] 本發(fā)明的目的是針對(duì)上述模型重構(gòu)方法的不足，提供一種基于投影幾何法的單關(guān) 節(jié)故障機(jī)械臂模型重構(gòu)方法，解決了單關(guān)節(jié)故障狀態(tài)下機(jī)械臂運(yùn)動(dòng)模型發(fā)生改變，無(wú)法進(jìn) 行運(yùn)動(dòng)控制的問(wèn)題。
[0005] 本發(fā)明所采用的技術(shù)方案是:一種基于投影幾何法的單關(guān)節(jié)故障機(jī)械臂模型重構(gòu) 方法，步驟如下：
[0006] 1)基于旋量坐標(biāo)表征機(jī)械臂的運(yùn)動(dòng)學(xué)模型，根據(jù)故障關(guān)節(jié)在基礎(chǔ)坐標(biāo)系下的軸向 不同，其故障角度影響各關(guān)節(jié)旋量坐標(biāo)的不同參數(shù)，將故障關(guān)節(jié)沿軸向進(jìn)行分類，針對(duì)具有 不同軸向的故障關(guān)節(jié)，采用投影法在其軸線的公垂面構(gòu)造投影平面，獲得運(yùn)動(dòng)學(xué)模型平面 投影圖；
[0007] 2)基于平面投影圖，采用幾何分析法確定機(jī)械臂的旋量坐標(biāo)，針對(duì)不同類關(guān)節(jié)發(fā) 生故障的情況，分析故障關(guān)節(jié)鎖定在固定角度后，基礎(chǔ)坐標(biāo)系與工具坐標(biāo)系的變換矩陣，以 及重構(gòu)運(yùn)動(dòng)學(xué)模型中各關(guān)節(jié)的旋量坐標(biāo)與故障角度的關(guān)系，實(shí)現(xiàn)單關(guān)節(jié)故障機(jī)械臂運(yùn)動(dòng)學(xué) 模型重構(gòu)。
[0008] 步驟1)投影法對(duì)機(jī)械臂運(yùn)動(dòng)學(xué)模型進(jìn)行二維投影包括以下步驟：
[0009] 11)規(guī)定關(guān)節(jié)故障狀態(tài)下的標(biāo)號(hào)規(guī)則，當(dāng)關(guān)節(jié)Jf鎖定時(shí)，除關(guān)節(jié)Jf外，其余關(guān)節(jié)的相 關(guān)符號(hào)均增加"~"，且其后關(guān)節(jié)標(biāo)號(hào)在原基礎(chǔ)上減一，即^ 丨二且機(jī) 械臂自由度數(shù)退化為m = n-l;
[0010] 12)針對(duì)關(guān)節(jié)在基礎(chǔ)坐標(biāo)系中的軸向不同，其故障角度對(duì)各關(guān)節(jié)旋量坐標(biāo)的影響 不同，將故障關(guān)節(jié)進(jìn)行分類，即X軸關(guān)節(jié)、Y軸關(guān)節(jié)、Z軸關(guān)節(jié)；
[0011] 13)基于旋量坐標(biāo)表征機(jī)械臂的運(yùn)動(dòng)學(xué)模型，規(guī)定除故障關(guān)節(jié)Jf鎖定在故障角度 以外，其余關(guān)節(jié)固定于6 =0 (/ = Ι,...,/ηΠ ./#/),根據(jù)故障關(guān)節(jié)Jf的軸向確定其運(yùn)動(dòng)學(xué)模型 的二維投影平面:x軸關(guān)節(jié)對(duì)應(yīng)基礎(chǔ)坐標(biāo)系的YZ平面投影圖、Y軸關(guān)節(jié)對(duì)應(yīng)XZ平面投影圖、Z 軸關(guān)節(jié)對(duì)應(yīng)XY平面投影圖，二維圖可用于確定各關(guān)節(jié)除故障關(guān)節(jié)軸向方向的旋量坐標(biāo)，沿 故障關(guān)節(jié)軸向方向的旋量坐標(biāo)需由重構(gòu)的運(yùn)動(dòng)學(xué)模型獲得。
[0012] 步驟2)幾何分析法確定旋量坐標(biāo)包括以下步驟：
[0013] 21)若故障關(guān)節(jié)為X軸關(guān)節(jié)，關(guān)節(jié)故障后，基礎(chǔ)坐標(biāo)系與工具坐標(biāo)系的變換關(guān)系為：
[0014]
[0015] 其中，0f表示故障關(guān)節(jié)鎖定的角度，Σ1Χ表示沿X軸方向連桿的長(zhǎng)度之和，Σ?ρ和 Σ lzr分別表不發(fā)生故障的關(guān)節(jié)之前的Υ軸和Ζ軸方向連桿的長(zhǎng)度之和，Σ lyf和Σ lzf分別表 示發(fā)生故障的關(guān)節(jié)之后的Y軸和Z軸方向連桿的長(zhǎng)度之和。
[0016] 故障關(guān)節(jié)標(biāo)號(hào)前的關(guān)節(jié)的旋量坐標(biāo)保持原有值不變，標(biāo)號(hào)后的關(guān)節(jié)的旋量坐標(biāo) 為：
[0017]
[0018]
[0019]
[0020]
[0021] 其中，命分別表示Y軸關(guān)節(jié)及Z軸關(guān)節(jié)的運(yùn)動(dòng)旋量軸線方向上的單位矢量，qi = (xi yi zi)表示故障前軸線上的點(diǎn)。
[0022] 22)若故障關(guān)節(jié)為Y軸關(guān)節(jié)，關(guān)節(jié)故障后，基礎(chǔ)坐標(biāo)系與工具坐標(biāo)系的變換關(guān)系為：
[0023]
[0024] 其中，Σ1Υ表示沿Y軸方向連桿的長(zhǎng)度之和，ΣΙχγ表示發(fā)生故障的關(guān)節(jié)之前的X軸 方向連桿的長(zhǎng)度之和，Σ1 χ?表示發(fā)生故障的關(guān)節(jié)之后的X軸方向連桿的長(zhǎng)度之和。
[0025] 故障關(guān)節(jié)標(biāo)號(hào)前的關(guān)節(jié)的旋量坐標(biāo)保持原有值不變，標(biāo)號(hào)后的關(guān)節(jié)的旋量坐標(biāo) 為：
[0026]
[0027]
[0028]
[0029]
[0030] 其中，氡表示X軸關(guān)節(jié)的運(yùn)動(dòng)旋量軸線方向上的單位矢量，I.表示關(guān)節(jié)i的運(yùn)動(dòng)旋 量。
[0031] 23)若故障關(guān)節(jié)為Z軸關(guān)節(jié)，關(guān)節(jié)故障后，基礎(chǔ)坐標(biāo)系與工具坐標(biāo)系的變換關(guān)系為：
[0032]
[0033] 故障關(guān)節(jié)標(biāo)號(hào)前的關(guān)節(jié)的旋量坐標(biāo)保持原有值不變，標(biāo)號(hào)后的關(guān)節(jié)的旋量坐標(biāo) 為：
[0034]
[0035]
[0036]
[0037]
[0038] 本發(fā)明與現(xiàn)有技術(shù)相比具有以下優(yōu)點(diǎn)：
[0039] (1)本發(fā)明通過(guò)旋量坐標(biāo)表征機(jī)械臂的運(yùn)動(dòng)學(xué)模型，實(shí)現(xiàn)了不同構(gòu)型機(jī)械臂單關(guān) 節(jié)失效后運(yùn)動(dòng)學(xué)模型重構(gòu)的通用性，并且在機(jī)械臂構(gòu)型復(fù)雜、自由度高時(shí)，其通用性尤為明 顯。
[0040] (2)本發(fā)明采用投影法在故障關(guān)節(jié)軸線的公垂面構(gòu)造投影平面，能夠有效地分析 關(guān)節(jié)鎖定在故障角度后，基礎(chǔ)坐標(biāo)系與工具坐標(biāo)系的變換矩陣，以及重構(gòu)運(yùn)動(dòng)學(xué)模型中各 關(guān)節(jié)的旋量坐標(biāo)與故障角度的關(guān)系。
[0041] (3)本發(fā)明采用幾何法推導(dǎo)出各類關(guān)節(jié)發(fā)生故障時(shí)的旋量坐標(biāo)與故障關(guān)節(jié)角度及 連桿長(zhǎng)度之間的數(shù)學(xué)表達(dá)式，在已知常態(tài)下機(jī)械臂旋量坐標(biāo)及故障關(guān)節(jié)鎖定角度的基礎(chǔ) 上，可直接獲得重構(gòu)的旋量坐標(biāo)，提高了運(yùn)動(dòng)學(xué)模型重構(gòu)的效率，能夠滿足在軌應(yīng)用的實(shí)時(shí) 性需求。
【附圖說(shuō)明】
[0042]圖1是本發(fā)明方法流程圖。
[0043]圖2是常態(tài)下旋量坐標(biāo)系。
[0044]圖3是關(guān)節(jié)三鎖定后的重建旋量系。
[0045]圖4是關(guān)節(jié)三鎖定后YZ平面投影圖。
[0046]圖5是關(guān)節(jié)二鎖定后的重建旋量系。
[0047]圖6是關(guān)節(jié)二鎖定后XZ平面投影圖。
[0048]圖7是關(guān)節(jié)一鎖定后的重建旋量系。
[0049] 圖8是關(guān)節(jié)一鎖定后XY平面投影圖。
【具體實(shí)施方式】
[0050] 1、投影法對(duì)單關(guān)節(jié)失效機(jī)械臂運(yùn)動(dòng)學(xué)模型進(jìn)行二維投影
[0051] 如圖1所示為本發(fā)明方法流程圖。本發(fā)明以空間七自由度機(jī)械臂作為研究對(duì)象，各 桿件長(zhǎng)度如表1所示，機(jī)械臂的構(gòu)型如圖2所示。對(duì)于每一個(gè)關(guān)節(jié)Ji(i = l，...，7)，構(gòu)造其運(yùn) 動(dòng)旋量，旋量坐標(biāo)如表2所示。
[0052] 表1連桿長(zhǎng)度
[0053]
-
[0054] 表2常態(tài)下旋量坐標(biāo)
[0055]
[0056] X軸關(guān)節(jié)J3鎖定在30°時(shí)，J3之后的關(guān)節(jié)標(biāo)記為$ -J6。重構(gòu)的旋量系如圖3所示，YZ 方向投影圖如圖4所示。Y軸關(guān)節(jié)J2鎖定在30°時(shí)，J2之后的關(guān)節(jié)標(biāo)記為..?-J6。重構(gòu)的旋量系 如圖5所示，ΧΖ方向投影圖如圖6所示。Ζ軸關(guān)節(jié)h鎖定在30°時(shí)，Ji之后的關(guān)節(jié)標(biāo)記為乂-人。 重構(gòu)的旋量系如圖7所示，XY方向投影圖如圖8所示。
[0057] 2、幾何分析法確定旋量坐標(biāo)
[0058] X軸關(guān)節(jié)J3鎖定時(shí)，產(chǎn)生變化的旋量參數(shù)為辱-4和％-，均為θ3的函數(shù)，重構(gòu)后 的旋量參數(shù)如表3所示，旋量坐標(biāo)如表4所示?；A(chǔ)坐標(biāo)系S保持原有方向，工具坐標(biāo)系Τ的方 向發(fā)生變化，基礎(chǔ)坐標(biāo)系與工具坐標(biāo)系的變換關(guān)系為：
[0059]
(1)
[0060] 表3三關(guān)節(jié)鎖定后重構(gòu)旋量參數(shù)
[0061]
[0062] 表4三關(guān)節(jié)鎖定后重構(gòu)旋量坐標(biāo)
[0063]
[0064] Y軸關(guān)節(jié)J2鎖定時(shí)，產(chǎn)生變化的旋量參數(shù)為氣-?和么，均為92的函數(shù)。重構(gòu)后 的旋量參數(shù)如表5所示，旋量坐標(biāo)如表6所示?；A(chǔ)坐標(biāo)系S保持原有方向，工具坐標(biāo)系T的方 向發(fā)生變化，基礎(chǔ)坐標(biāo)系與工具坐標(biāo)系的變換關(guān)系為：
[0065]
(2)
[0066]表5二關(guān)節(jié)鎖定后重構(gòu)旋量參數(shù)
[0067]
[0068] Z軸關(guān)節(jié)Ji鎖定時(shí)，產(chǎn)生變化的旋量參數(shù)為兩-爲(wèi)和條-貳，均為Θ:的函數(shù)。重構(gòu)后 的旋量參數(shù)如表7所示，旋量坐標(biāo)如表8所示。基礎(chǔ)坐標(biāo)系S保持原有方向，工具坐標(biāo)系T的方 向發(fā)生變化，基礎(chǔ)坐標(biāo)系與工具坐標(biāo)系的變換關(guān)系為：
[0069]
(3)
[0070] 表6二關(guān)節(jié)鎖定后重構(gòu)旋量坐標(biāo)
[0071]
[0072]表7-關(guān)節(jié)鎖定后重構(gòu)旋量參數(shù)
[0073]
[0074] 表8-關(guān)節(jié)鎖定后重構(gòu)旋量坐標(biāo)
[0075]
[0076] 本發(fā)明說(shuō)明書(shū)中未作詳細(xì)描述的內(nèi)容屬本領(lǐng)域技術(shù)人員的公知技術(shù)。
【主權(quán)項(xiàng)】
1. 一種基于投影幾何法的單關(guān)節(jié)故障機(jī)械臂模型重構(gòu)方法，其特征在于該方法包括以 下步驟： 1) 基于旋量坐標(biāo)表征機(jī)械臂的運(yùn)動(dòng)學(xué)模型，根據(jù)故障關(guān)節(jié)在基礎(chǔ)坐標(biāo)系下的軸向不 同，其故障角度影響各關(guān)節(jié)旋量坐標(biāo)的不同參數(shù)，將故障關(guān)節(jié)沿軸向進(jìn)行分類，針對(duì)具有不 同軸向的故障關(guān)節(jié)，采用投影法在其軸線的公垂面構(gòu)造投影平面，獲得運(yùn)動(dòng)學(xué)模型平面投 影圖； 2) 基于平面投影圖，采用幾何分析法確定機(jī)械臂的旋量坐標(biāo)，針對(duì)不同類關(guān)節(jié)發(fā)生故 障的情況，分析故障關(guān)節(jié)鎖定在固定角度后，基礎(chǔ)坐標(biāo)系與工具坐標(biāo)系的變換矩陣，以及重 構(gòu)運(yùn)動(dòng)學(xué)模型中各關(guān)節(jié)的旋量坐標(biāo)與故障角度的關(guān)系，實(shí)現(xiàn)單關(guān)節(jié)故障機(jī)械臂運(yùn)動(dòng)學(xué)模型 重構(gòu)。2. 根據(jù)權(quán)利要求1所述的一種基于投影幾何法的單關(guān)節(jié)故障機(jī)械臂模型重構(gòu)方法，其 特征在于步驟1)投影法對(duì)機(jī)械臂運(yùn)動(dòng)學(xué)模型進(jìn)行二維投影包括以下步驟： 11) 針對(duì)關(guān)節(jié)在基礎(chǔ)坐標(biāo)系中的軸向不同，其故障角度對(duì)各關(guān)節(jié)旋量坐標(biāo)的影響不同， 將故障關(guān)節(jié)進(jìn)行分類，即X軸關(guān)節(jié)、Y軸關(guān)節(jié)、Z軸關(guān)節(jié)； 12) 基于旋量坐標(biāo)表征機(jī)械臂的運(yùn)動(dòng)學(xué)模型，規(guī)定除故障關(guān)節(jié)Jf鎖定在故障角度以外， 其余關(guān)節(jié)固定于肩_=_〇(i = l，. . .，m且i矣f)，根據(jù)故障關(guān)節(jié)Jf的軸向確定其運(yùn)動(dòng)學(xué)模型的 二維投影平面:X軸關(guān)節(jié)對(duì)應(yīng)基礎(chǔ)坐標(biāo)系的YZ平面投影圖、Y軸關(guān)節(jié)對(duì)應(yīng)XZ平面投影圖、Z軸 關(guān)節(jié)對(duì)應(yīng)XY平面投影圖，二維圖可用于確定各關(guān)節(jié)除故障關(guān)節(jié)軸向方向的旋量坐標(biāo)，沿故 障關(guān)節(jié)軸向方向的旋量坐標(biāo)需由重構(gòu)的運(yùn)動(dòng)學(xué)模型獲得。3. 根據(jù)權(quán)利要求1所述的一種基于投影幾何法的單關(guān)節(jié)故障機(jī)械臂模型重構(gòu)方法，其 特征在于步驟2)幾何分析法確定旋量坐標(biāo)包括以下步驟： 21) 若故障關(guān)節(jié)為X軸關(guān)節(jié)，關(guān)節(jié)故障后，基礎(chǔ)坐標(biāo)系與工具坐標(biāo)系的變換關(guān)系為：其中，0f表示故障關(guān)節(jié)鎖定的角度，Σ lx表示沿X軸方向連桿的長(zhǎng)度之和，Σ lyr和Σ lzr 分別表不發(fā)生故障的關(guān)節(jié)之前的Y軸和Z軸方向連桿的長(zhǎng)度之和，Σ lyf和Σ lZf分別表不發(fā) 生故障的關(guān)節(jié)之后的Y軸和Z軸方向連桿的長(zhǎng)度之和。 故障關(guān)節(jié)標(biāo)號(hào)前的關(guān)節(jié)的旋量坐標(biāo)保持原有值不變，標(biāo)號(hào)后的關(guān)節(jié)的旋量坐標(biāo)為：其中，今電分別表示γ軸關(guān)節(jié)及z軸關(guān)節(jié)的運(yùn)動(dòng)旋量軸線方向上的單位矢量，qi=(Xl Yl Zi)表示故障前軸線上的點(diǎn)。 22) 若故障關(guān)節(jié)為Y軸關(guān)節(jié)，關(guān)節(jié)故障后，基礎(chǔ)坐標(biāo)系與工具坐標(biāo)系的變換關(guān)系為：其中，Σ ly表示沿Y軸方向連桿的長(zhǎng)度之和，Σ lxr表示發(fā)生故障的關(guān)節(jié)之前的X軸方向 連桿的長(zhǎng)度之和，Σ1χ?表示發(fā)生故障的關(guān)節(jié)之后的X軸方向連桿的長(zhǎng)度之和。 故障關(guān)節(jié)標(biāo)號(hào)前的關(guān)節(jié)的旋量坐標(biāo)保持原有值不變，標(biāo)號(hào)后的關(guān)節(jié)的旋量坐標(biāo)為：其中，4表示X軸關(guān)節(jié)的運(yùn)動(dòng)旋量軸線方向上的單位矢量。 23)若故障關(guān)節(jié)為Ζ軸關(guān)節(jié)，關(guān)節(jié)故障后，基礎(chǔ)坐標(biāo)系與工具坐標(biāo)系的變換關(guān)系為： 0 0 01 故障關(guān)節(jié)標(biāo)號(hào)前的關(guān)節(jié)的旋量坐標(biāo)保持原有值不變，標(biāo)號(hào)后的關(guān)節(jié)的旋量坐標(biāo)為：
【文檔編號(hào)】B25J9/16GK105856240SQ201610413044
【公開(kāi)日】2016年8月17日
【申請(qǐng)日】2016年6月14日
【發(fā)明人】賈慶軒, 郭雯, 陳鋼, 李彤, 徐濤, 王玉琦
【申請(qǐng)人】北京郵電大學(xué)