[發明專利]一種基于附加編碼器的并聯六軸機器人速度正解方法

在審

• 申請號: 202210395720.7
• 申請日: 2022-04-15
• 公開（公告）號: CN114589700A
• 公開（公告）日: 2022-06-07
• 發明（設計）人: 郭鵬
• 申請（專利權）人: 伯朗特機器人股份有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 廣州駿思知識產權代理有限公司 44425
• 代理人: 龍婷

1.一種基于附加編碼器的并聯六軸機器人速度正解方法，其特征在于，

所述六軸機器人包括靜平臺、動平臺、虎克鉸組件、關節組件和電缸組件；

所述關節組件包括：

軸承座；

交叉軸殼體，所述交叉軸殼體轉動設置在所述軸承座上；

軸耳，所述軸耳轉動設置在所述交叉軸殼體上，所述軸耳在所述交叉軸殼體上的旋轉軸線與所述交叉軸殼體在所述軸承座上的旋轉軸線相交且相互垂直，在所述軸耳上設置有兩個軸承孔，所述兩個軸承孔關于所述軸耳在所述交叉軸殼體上的旋轉軸線對稱分布，所述兩個軸承孔的軸線相互平行且位于同一水平面上，所述軸承孔的軸線與所述交叉軸殼體在所述軸承座上的旋轉軸線相互垂直；

所述虎克鉸組件安裝在所述靜平臺上，所述關節組件安裝在所述動平臺上，所述電缸組件的一端安裝在所述虎克鉸組件上，所述電缸組件的另一端通過承力銷軸轉動連接在所述關節組件的所述軸耳上，所述承力銷軸轉動設置在所述軸耳的所述軸承孔內；

在所述靜平臺上安裝有六個所述虎克鉸組件，所述六個虎克鉸組件分別為第一虎克鉸組件、第二虎克鉸組件、第三虎克鉸組件、第四虎克鉸組件、第五虎克鉸組件、第六虎克鉸組件，在所述動平臺上設置有三個所述關節組件，所述三個關節組件分別為第一關節組件、第二關節組件、第三關節組件，所述靜平臺與所述動平臺之間設置有六個所述電缸組件，所述六個電缸組件分別為第一電缸組件、第二電缸組件、第三電缸組件、第四電缸組件、第五電缸組件、第六電缸組件；

所述第二電缸組件、所述第三電缸組件、所述第二虎克鉸組件和所述第三虎克鉸組件以及所述第一關節組件形成第一控制組件，所述第四電缸組件、所述第五電缸組件、所述第四虎克鉸組件、所述第五虎克鉸組件以及所述第二關節組件形成第二控制組件，所述第六電缸組件、所述第一電缸組件、所述第六虎克鉸組件、所述第一虎克鉸組件、以及所述第三關節組件形成第三控制組件；

所述速度正解方法包括：

S10：在機器人的第一控制組件上安裝五個角度編碼器以讀取關節角度值和關節角速度值，包括如下子步驟：

S101：在第一控制組件的第二虎克鉸組件的虎克鉸上基座和虎克鉸下基座上均安裝一個角度編碼器，記為第一角度編碼器和第二角度編碼器，用于測量第二虎克鉸組件的固定軸部和擺動軸部分別相對于虎克鉸下基座和虎克鉸上基座的旋轉角度和旋轉角速度，分別記為jθ₁、jθ₂和

S102：在第一控制組件的第三虎克鉸組件的虎克鉸上基座上安裝一角度編碼器，記為第三角度編碼器，用于測量擺動軸部相對于虎克鉸組件的虎克鉸上基座的旋轉角度和旋轉角速度，記為jθ₃和

S103：在第一控制組件的關節組件內部安裝一角度編碼器，記為第四角度編碼器，用于測量關節組件的軸耳的軸部相對于交叉軸殼體的角度值和角速度值，記為jθ₄和

S104：在動平臺上與第一控制組件連接的位置處安裝一角度編碼器，記為第五角度編碼器，用于測量動平臺相對于關節組件的旋轉角度值和旋轉角速度值，記為jθ₅和

S20：構建并聯六軸機器人的運動學元素；

B_i,i＝1～6：虎克鉸組件原點，將B_i,i＝1～6點設置在虎克鉸組件的十字軸中心；

{O}：基坐標系，在靜平臺上建立直角坐標系{O}，將坐標系的原點O設置在B₁～B₆確定的平面上，且位于B₁～B₆確定的圓的圓心位置，y軸方向設置在OB₁,OB₂線段的角平分線位置，此時六個虎克鉸組件相對于y軸對稱，z軸設置朝上，x軸根據右手定則即可自動確定；

R_b：表示B₁～B₆確定的圓的半徑，稱為“虎克鉸虛擬圓半徑”；

^OOB₁～^OOB₆：^OOB_i：虎克鉸位置向量，具體表示以基坐標系的原點為起點，第i個虎克鉸原點B_i,i＝1～6為終點的向量，參考坐標系為基坐標系；

β_i，i＝1～6：虎克鉸偏置角，表示^OOB_i與基坐標系{O}的Y軸的角度；

R_Q：稱為“交叉軸虛擬圓半徑”，表示三個交叉軸原點確定的圓的半徑值；

Q_i，i＝1～3：軸耳原點，軸耳零件上與U_i，i＝1～3對應重合的點，設為Q_i，i＝1～3；

{Q_i}，i＝1～3：表示軸耳坐標系。坐標系原點固連在軸耳原點Q_i，{Q_i}的z軸沿軸耳的軸部軸線方向，y軸沿銷軸鉸鏈兩點的連線方向，x軸根據右手定則確定；

U_i：交叉軸原點，關節組件的兩個圓柱面的軸線行成一個交點，設該點為U_i，i＝1～3；

{U_i}和i＝1～3：分別表示交叉軸坐標系，坐標系原點固連在交叉軸零件原點U_i，{U_i}的z軸沿軸耳的軸部軸線方向，y軸沿中心軸軸線的方向，x軸根據右手定則確定。

{P}：動平臺坐標系，{P}的原點P位于U_i，i＝1～3確定的平面上，且位于三個U_i確定的圓的圓心位置。設置{P}的y軸設置為：其中U₂在y軸的負方向，U₁,U₃相對于y軸對稱，z軸朝上，x軸可用右手定則確定。如圖13所示。

軸耳原點的分布角度

^OOP：動平臺的位置，表示動平臺坐標系{P}的原點P相對于靜平臺坐標系原點O的位置向量；

^OR_P：動平臺的姿態，表示動平臺坐標系{P}相對于靜平臺坐標系{O}的旋轉矩陣；

A₁～A₆：銷軸鉸鏈中心，在軸耳零件1(軸耳零件2，軸耳零件3)中，兩個銷軸鉸鏈中心點A₂(點A₄，點A₆)和點A₃(點A₅，點A₁)點相對于{Q₁}({Q₂}，{Q₃})的Y軸對稱布置，將點A₂(點A₄，點A₆)和點A₃(點A₅，點A₁)放置在{Q₁}({Q₂}，{Q₃})的XY平面上，用QAX表示A₂和A₃在{Q₁}的X軸方向的絕對偏移值，QAY表示A₂和A₃在{Q₁}的Y軸方向的絕對偏移值；

^ΟΒΑ_i：表示電缸位置向量，以基坐標系{O}為參考系；

Q₁'：表示軸耳零件的速度瞬心；

S30：從電缸組件自帶的控制系統讀出電缸的長度，記為l1～l6，用l1～l6表示^ΟΒΑ₁～^ΟΒΑ₆的模，設l1＝|^ΟΒΑ₁|，...，l6＝|^ΟΒΑ₆|，從電缸組件自帶的控制系統讀出電缸組件中的活塞桿相對于缸體的伸縮速度v_Li,i＝1～6；

S40：在第二虎克鉸組件的十字軸上，以十字軸中心B₂為原點建立虎克鉸坐標系{B2}，在第三虎克鉸組件的十字軸上，以十字軸中心B₃為原點建立虎克鉸坐標系{B3}；

S50：在第二虎克鉸組件的十字軸上，建立電缸坐標系{L2}，其原點與{B2}的原點重合，在第三虎克鉸組件的虎克鉸上基座上，建立電缸坐標系{L3}，其原點與{B3}的原點重合；

S60：計算第二虎克鉸組件的中心B₂與第三虎克鉸組件的中心B₃的距離b₂₃；

S70：以{O}為參考系，求解{B2}、{B3}坐標系的位姿；

S80：以{O}為參考系，求解{L2}、{L3}坐標系的位姿；

S90：以{O}為參考系，求解A₂、A₃點的位置；

S100：求解{Q₁}相對于{O}的位姿，用^OT_Q1表示；

S110：求解{U₁}相對于{O}的位姿，用^OT_U1表示；

S120：求解{P₁}相對于{O}的位姿，用^OT_P1表示；

S130：求解動平臺的位姿，即動平臺的位置^OOP和動平臺的位姿^OR_P；

S140：以{O}為參考坐標系，求解第二虎克鉸組件和第三虎克鉸組件的十字軸相對于靜平臺的角速度向量，記作^OW_B2,O和^OW_B3,O；

S150：以{O}為參考坐標系，求解第二電缸組件和第三電缸組件的缸體相對于靜平臺的角速度向量，記作^OW_L2,O和^OW_L3,O；

S160：求解銷軸鉸鏈中心A₂，A₃的線速度向量，以{O}為參考坐標系，用^OV_A2和^OV_A3表示；

S170：求解軸耳零件上的軸耳原點Q₁點的線速度^OV_Q1，以及軸耳的角速度向量^OW_Q1,O；

S180：求解第一關節組件的交叉軸的角速度，以{O}為參考系，記作^OW_U1,O；

S190：求解動平臺的角速度，以{O}為參考系，即^OW_P,O；

S200：求解動平臺中心的線速度，以{O}為參考系，即^OV_P。