[發(fā)明專利]用于具有三角形遠(yuǎn)心點機構(gòu)的串聯(lián)機械臂逆運動學(xué)數(shù)值解法

有效

• 申請?zhí)枺?/td>: 201510975500.1
• 申請日: 2015-12-22
• 公開（公告）號: CN105629880B
• 公開（公告）日: 2018-09-07
• 發(fā)明（設(shè)計）人: 潘博;付宜利;曲效鋒;王樹國;艾躍
• 申請（專利權(quán)）人: 哈爾濱工業(yè)大學(xué)
• 主分類號: G05B19/408
• 分類號: G05B19/408
• 代理公司: 哈爾濱市松花江專利商標(biāo)事務(wù)所 23109
• 代理人: 牟永林

1.用于具有三角形遠(yuǎn)心點機構(gòu)的串聯(lián)機械臂逆運動學(xué)數(shù)值解法，其特征在于：所述用于具有三角形遠(yuǎn)心點機構(gòu)的串聯(lián)機械臂逆運動學(xué)數(shù)值解法的具體步驟如下：

步驟一、假設(shè)機械臂末端點與腕部起點O₄重合，即忽略手腕處三個關(guān)節(jié)，這是假設(shè)A；

步驟二、根據(jù)假設(shè)A，輸入量末端點位姿矩陣的位置分量與O₀坐標(biāo)系下的遠(yuǎn)心點位置矢量相減即為O₀坐標(biāo)系下的第三關(guān)節(jié)的位移量q₃的空間矢量，求出第三關(guān)節(jié)位移量q₃的中間計算值，命名為q₁₃；

步驟三、根據(jù)假設(shè)A，由移動第三關(guān)節(jié)的軸線方向可以唯一地確定第一關(guān)節(jié)位移量q₁和第二關(guān)節(jié)位移量q₂的大小，依次求出第一關(guān)節(jié)中間計算值q₁₁和第二關(guān)節(jié)中間計算值q₁₂；

步驟四、在不忽略第四關(guān)節(jié)、第五關(guān)節(jié)、第六關(guān)節(jié)的情況下，利用輸入量末端點位姿矩陣的姿態(tài)信息和根據(jù)假設(shè)A求出的第一關(guān)節(jié)中間計算值q₁₁、第二關(guān)節(jié)中間計算值q₁₂、第三關(guān)節(jié)中間計算值q₁₃依次求出第四關(guān)節(jié)中間計算值q₁₄、第五關(guān)節(jié)中間計算值q₁₅、第六關(guān)節(jié)中間計算值q₁₆；

步驟五、假設(shè)第四關(guān)節(jié)真實值q₀₄、第五關(guān)節(jié)真實值q₀₅、第六關(guān)節(jié)真實值q₀₆與剛計算出的第四關(guān)節(jié)中間計算值q₁₄、第五關(guān)節(jié)中間計算值q₁₅、第六關(guān)節(jié)中間計算值q₁₆相等，這是假設(shè)B；

步驟六、在第二關(guān)節(jié)坐標(biāo)系O₂下，根據(jù)假設(shè)B，用第三關(guān)節(jié)中間計算值q₁₃、第四關(guān)節(jié)中間計算值q₁₄、第五關(guān)節(jié)中間計算值q₁₅、第六關(guān)節(jié)中間計算值q₁₆求出第三關(guān)節(jié)新計算值q₂₃；

步驟七、通過第三關(guān)節(jié)新計算值q₂₃和第四關(guān)節(jié)中間計算值q₁₄、第五關(guān)節(jié)中間計算值q₁₅、第六關(guān)節(jié)中間計算值q₁₆依次求出第一關(guān)節(jié)新計算值q₂₁和第二關(guān)節(jié)新計算值q₂₂；

步驟八、通過第一關(guān)節(jié)新計算值q₂₁、第二關(guān)節(jié)新計算值q₂₂、第三關(guān)節(jié)新計算值q₂₃依次求出第四關(guān)節(jié)新計算值q₂₄、第五關(guān)節(jié)新計算值q₂₅、第六關(guān)節(jié)新計算值q₂₆；

步驟九、用第一關(guān)節(jié)新計算值q₂₁、第二關(guān)節(jié)新計算值q₂₂、第三關(guān)節(jié)新計算值q₂₃、第四關(guān)節(jié)新計算值q₂₄、第五關(guān)節(jié)新計算值q₂₅、第六關(guān)節(jié)新計算值q₂₆替代第一關(guān)節(jié)中間計算值q₁₁、第二關(guān)節(jié)中間計算值q₁₂、第三關(guān)節(jié)中間計算值q₁₃、第四關(guān)節(jié)中間計算值q₁₄、第五關(guān)節(jié)中間計算值q₁₅、第六關(guān)節(jié)中間計算值q₁₆，若未達(dá)到事先設(shè)定好的循環(huán)次數(shù)，則由步驟五重新進(jìn)入循環(huán)，否則，退出循環(huán)；

步驟十、輸出新的一組六個關(guān)節(jié)的位移值。