[發(fā)明專利]基于內(nèi)部傳感信息的混聯(lián)機器人運動學(xué)標(biāo)定方法

在審

• 申請?zhí)枺?/td>: 202211337039.3
• 申請日: 2022-10-28
• 公開（公告）號: CN115533876A
• 公開（公告）日: 2022-12-30
• 發(fā)明（設(shè)計）人: 田文杰;郭旭;王麗娜;張相鵬;黃田
• 申請（專利權(quán)）人: 天津大學(xué)
• 主分類號: B25J9/00
• 分類號: B25J9/00;B25J9/16
• 代理公司: 天津市北洋有限責(zé)任專利代理事務(wù)所 12201
• 代理人: 吳學(xué)穎

1.一種基于內(nèi)部傳感信息的混聯(lián)機器人運動學(xué)標(biāo)定方法，所述混聯(lián)機器人由并聯(lián)機構(gòu)與串聯(lián)轉(zhuǎn)頭構(gòu)成，所述并聯(lián)機構(gòu)由第一主動伸縮裝置(1)、第二主動伸縮裝置(2)、第三主動伸縮裝置(3)、從動伸縮裝置(4)、動平臺(5)、第一轉(zhuǎn)動支撐結(jié)構(gòu)(6)、第二轉(zhuǎn)動支撐結(jié)構(gòu)(7)、第一機架軸承座(8)、第二機架軸承座(9)組成，所述串聯(lián)轉(zhuǎn)頭由第一轉(zhuǎn)動部件(11)、第二轉(zhuǎn)動部件(12)、主軸(13)、刀具(14)構(gòu)成，其特征在于，標(biāo)定方法包括以下步驟：

步驟一，混聯(lián)機器人內(nèi)部傳感系統(tǒng)的設(shè)計、安裝與校準(zhǔn)

1)混聯(lián)機器人運動系統(tǒng)的劃分

依據(jù)運動學(xué)等效原理，將所述混聯(lián)機器人劃分為主動運動鏈與從動運動鏈兩個獨立的運動系統(tǒng)；所述主動運動鏈由第一主動伸縮裝置(1)、第二主動伸縮裝置(2)、第三主動伸縮裝置(3)、動平臺(5)、第一轉(zhuǎn)動支撐結(jié)構(gòu)(6)、第二轉(zhuǎn)動支撐結(jié)構(gòu)(7)、串聯(lián)轉(zhuǎn)頭構(gòu)成；所述從動運動鏈由第一轉(zhuǎn)動支撐結(jié)構(gòu)(6)、從動伸縮裝置(4)、動平臺(5)、串聯(lián)轉(zhuǎn)頭構(gòu)成；

2)內(nèi)部傳感系統(tǒng)的設(shè)計與安裝

依據(jù)內(nèi)部傳感器所對應(yīng)的運動生成元可張成末端運動輸出構(gòu)件可控運動空間的原則，在從動運動鏈上設(shè)計并安裝內(nèi)部傳感系統(tǒng)；所述內(nèi)部傳感系統(tǒng)由第一圓光柵(21)、第二圓光柵(22)、第三直線光柵(23)、第四圓光柵、第五圓光柵構(gòu)成，其中，第一圓光柵(21)安裝于第一轉(zhuǎn)動支撐結(jié)構(gòu)(6)與第一機架軸承座(8)之間的轉(zhuǎn)動副，第二圓光柵(22)安裝于第一轉(zhuǎn)動支撐結(jié)構(gòu)(6)與從動伸縮裝置(4)之間的轉(zhuǎn)動副，第三直線光柵(23)安裝于從動伸縮裝置(4)中的移動副，第四圓光柵安裝于第一轉(zhuǎn)動部件(11)與動平臺(5)之間的轉(zhuǎn)動副內(nèi)部，第五圓光柵安裝于第一轉(zhuǎn)動部件(11)與第二轉(zhuǎn)動部件(12)之間的轉(zhuǎn)動副內(nèi)部；

3)內(nèi)部傳感系統(tǒng)的校準(zhǔn)

采用外部測量儀器獲取混聯(lián)機器人運動至作業(yè)空間內(nèi)不同位形處的刀具六維位姿誤差，利用驅(qū)動關(guān)節(jié)誤差傳遞模型構(gòu)建誤差傳遞方程組，并利用正則回歸算法求解出機器人五個驅(qū)動關(guān)節(jié)的零點誤差；將所得零點誤差的相反數(shù)存儲至數(shù)控系統(tǒng)中與各個驅(qū)動關(guān)節(jié)對應(yīng)的零點偏置變量中，實現(xiàn)混聯(lián)機器人的驅(qū)動關(guān)節(jié)零點誤差補償；在此基礎(chǔ)上，控制混聯(lián)機器人各驅(qū)動關(guān)節(jié)運動至零點位置，并將當(dāng)前位形下的各個光柵的讀數(shù)清零，定義此時的各光柵位置為各光柵的零點位置，從而實現(xiàn)內(nèi)部傳感系統(tǒng)的零位校準(zhǔn)；

步驟二，混聯(lián)機器人從動運動鏈結(jié)構(gòu)誤差參數(shù)的識別

1)主動運動鏈與從動運動鏈之間的理想運動學(xué)關(guān)系的構(gòu)建

采用機構(gòu)學(xué)方法建立無誤差狀態(tài)下主動運動鏈驅(qū)動關(guān)節(jié)變量與刀具位姿之間的映射模型p＝f_主(q_主)，其中，p為刀具六維位姿向量，q_主為主動運動鏈驅(qū)動關(guān)節(jié)變量，f_主為q_主與p之間的映射函數(shù)；采用機構(gòu)學(xué)方法建立無誤差狀態(tài)下從動運動鏈內(nèi)部傳感系統(tǒng)讀數(shù)與刀具位姿之間的映射模型其中，q_從為從動運動鏈廣義驅(qū)動關(guān)節(jié)變量，其數(shù)值由從動運動鏈內(nèi)部傳感系統(tǒng)讀數(shù)獲取，為p與q_從之間的映射函數(shù)；

聯(lián)立上述兩個函數(shù)關(guān)系式，求解得到無誤差狀態(tài)下主動運動鏈驅(qū)動關(guān)節(jié)變量q_主與從動運動鏈廣義驅(qū)動關(guān)節(jié)變量q_從之間的映射函數(shù)：

2)從動運動鏈綜合誤差模型的建立

依據(jù)從動運動鏈的誤差源是否可被內(nèi)部傳感系統(tǒng)直接檢測，將從動運動鏈中的誤差源分解為廣義驅(qū)動關(guān)節(jié)運動誤差Δq_從與結(jié)構(gòu)誤差Δs兩類；所述廣義驅(qū)動關(guān)節(jié)運動誤差Δq_從是指與內(nèi)部傳感系統(tǒng)測量量值相對應(yīng)的關(guān)節(jié)運動誤差，通過從動運動鏈廣義驅(qū)動關(guān)節(jié)變量的實測值q_從,實測與理論值q_從,理論作差得到，即Δq_從＝q_從,實測-q_從,理論，q_從,理論由式(1)計算得到；所述結(jié)構(gòu)誤差Δs包括從動運動鏈的零部件加工誤差、裝配誤差、軸線偏差；

采用旋量方法、POE方法或D-H方法，建立線性化表達的從動運動鏈綜合誤差模型如下：

Δp＝AΔq_從+BΔs (2)

式中，Δp為刀具位姿誤差向量，A為廣義驅(qū)動關(guān)節(jié)運動誤差映射矩陣，B為結(jié)構(gòu)誤差映射矩陣；

3)刀具位姿誤差的測量與求解

當(dāng)機器人運動至任一位形時，利用激光跟蹤儀儀器設(shè)備測量主軸上不共線的3個點P₁、P₂、P₃的空間位置誤差，分別記為Δr_P,1、Δr_P,2、Δr_P,3；將點P_m(m＝1,2,3)的位置誤差Δr_P,m表示為

Δr_P,m＝Δr+Δθ×Rr_P,m,0，m＝1,2,3 (3)

式中，Δr與Δθ分別為刀具的位置與姿態(tài)誤差向量，r_P,m,0為刀尖點指向點P_m的向量，R為刀具相對于混聯(lián)機器人機架的姿態(tài)矩陣；

將式(3)中的三個等式聯(lián)立并改寫成矩陣格式，有

Δr_P＝KΔp (4)

式中，Δr_P為由三個點的位置誤差構(gòu)成的向量，Δp為刀具位姿誤差向量，K為Δr_P與Δp之間的映射矩陣，I₃為三階單位矩陣；根據(jù)式(4)并采用最小二乘方法，求解機器人位于當(dāng)前位形處的刀具位姿誤差向量Δp

Δp＝(K^TK)^-1K^TΔr_P (5)

式中，K^T為矩陣K的轉(zhuǎn)置矩陣；

4)從動運動鏈結(jié)構(gòu)誤差的辨識

控制機器人運動至其工作空間中的n個位形，在每一個位形處，利用所述刀具位姿誤差的測量與求解方法，獲取刀具的位姿誤差向量Δp，并記第i個位形處的機器人刀具位姿誤差向量為Δp_i(i＝1～n)；利用n個位形對應(yīng)的n個從動運動鏈綜合誤差模型(即公式(2))組集成從動運動鏈結(jié)構(gòu)誤差的辨識方程組：

其中，Δp_i為第i個位形處的機器人刀具位姿誤差向量，A_i為第i個位形處的從動運動鏈廣義驅(qū)動關(guān)節(jié)運動誤差映射矩陣，B_i為第i個位形處的從動運動鏈結(jié)構(gòu)誤差映射矩陣，Δq_從,i為第i個位形處的從動運動鏈廣義驅(qū)動關(guān)節(jié)運動誤差，可由從動運動鏈內(nèi)部傳感系統(tǒng)讀數(shù)計算得到；

所選取的n個位形必須使得從動運動鏈結(jié)構(gòu)誤差Δs可辨識，利用式(6)并采用最小二乘法辨識出Δs，求解方法為

式中，矩陣右上角的字符T表示矩陣的轉(zhuǎn)置；

步驟三，基于從動運動鏈內(nèi)部傳感系統(tǒng)實時測量信息的機器人位姿誤差補償

1)混聯(lián)機器人末端構(gòu)件位姿誤差的實時預(yù)測

當(dāng)混聯(lián)機器人運動至其工作空間中任一位形處時，首先利用式(1)計算得到從動運動鏈廣義驅(qū)動關(guān)節(jié)變量的理論值q_從,理論，同時利用從動運動鏈內(nèi)部傳感系統(tǒng)讀取從動運動鏈廣義驅(qū)動關(guān)節(jié)變量實測值q_從,實測，進而求解出廣義驅(qū)動關(guān)節(jié)運動誤差Δq_從＝q_從,實測-q_從,理論；

利用混聯(lián)機器人運動學(xué)模型以及從動運動鏈綜合誤差模型，求解當(dāng)前位形處的從動運動鏈廣義驅(qū)動關(guān)節(jié)運動誤差映射矩陣A以及從動運動鏈結(jié)構(gòu)誤差映射矩陣B，并將步驟二中辨識得到的從動運動鏈結(jié)構(gòu)誤差Δs以及實時求解得到的從動運動鏈廣義驅(qū)動關(guān)節(jié)運動誤差Δq_從代入式(2)所述從動運動鏈綜合誤差模型，即實時求得混聯(lián)機器人刀具位姿誤差向量Δp；

2)混聯(lián)機器人驅(qū)動關(guān)節(jié)補償量的實時求解

將求解得到的刀具位姿誤差向量Δp以及理想刀具位姿向量p_理想求和，得到刀具真實位姿向量p_實際；將p_實際代入步驟二中所述無誤差狀態(tài)下主動運動鏈驅(qū)動關(guān)節(jié)變量與刀具位姿之間的映射模型，求解得到考慮位姿誤差狀態(tài)下的主動運動鏈驅(qū)動關(guān)節(jié)變量q′_主，計算方法如下：

式中，為混聯(lián)機器人主動運動鏈的理想運動學(xué)逆解函數(shù)；

將主動運動鏈理想驅(qū)動關(guān)節(jié)變量q_主與考慮位姿誤差狀態(tài)下的主動運動鏈驅(qū)動關(guān)節(jié)變量q′_主作差，得到混聯(lián)機器人驅(qū)動關(guān)節(jié)實時補償量Δq_主,補償，計算方法如下：

Δq_主,補償＝q_主-q′_主 (9)

3)基于內(nèi)部傳感信息的混聯(lián)機器人位姿誤差實時補償

控制混聯(lián)機器人主動運動鏈驅(qū)動關(guān)節(jié)運動至q_主+Δq_主,補償，利用從動運動鏈內(nèi)部傳感系統(tǒng)讀取當(dāng)前狀態(tài)下的從動運動鏈廣義驅(qū)動關(guān)節(jié)變量實測值，并計算當(dāng)前狀態(tài)下的廣義驅(qū)動關(guān)節(jié)運動誤差Δq_從；如果Δq_從的2-范數(shù)||Δq_從||小于或等于給定的閾值ε，則當(dāng)前位形下的實時補償過程結(jié)束；如果Δq_從的2-范數(shù)||Δq_從||大于給定的閾值ε，則重復(fù)執(zhí)行步驟三所述流程，直至精度達標(biāo)。