[發(fā)明專利]四自由度串聯(lián)機(jī)器人逆運動學(xué)求解方法和系統(tǒng)

有效

• 申請?zhí)枺?/td>: 202110341066.7
• 申請日: 2021-03-30
• 公開（公告）號: CN113510690B
• 公開（公告）日: 2022-07-26
• 發(fā)明（設(shè)計）人: 曾遠(yuǎn)帆;唐曉峰;李加寶;殷翔;范文晶;湯日佳;韓銳;張玉鑫
• 申請（專利權(quán)）人: 上海機(jī)電工程研究所
• 主分類號: B25J9/08
• 分類號: B25J9/08;B25J9/16;B25J17/02;B25J18/02
• 代理公司: 上海漢聲知識產(chǎn)權(quán)代理有限公司 31236
• 代理人: 胡晶

1.一種四自由度串聯(lián)機(jī)器人逆運動學(xué)求解方法，其特征在于，包括如下步驟：

步驟一：建立各連桿坐標(biāo)系并確定各關(guān)節(jié)的D-H運動學(xué)參數(shù)，所述四自由度串聯(lián)機(jī)器人由四個連桿串聯(lián)構(gòu)成四個關(guān)節(jié)，依次為：回轉(zhuǎn)臂-關(guān)節(jié)1、大臂-關(guān)節(jié)2、外節(jié)臂-關(guān)節(jié)3和伸縮臂-關(guān)節(jié)4，其中關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3為旋轉(zhuǎn)關(guān)節(jié)，關(guān)節(jié)4為平移關(guān)節(jié)；

步驟二：求機(jī)器人的運動學(xué)正解，得到由各關(guān)節(jié)運動學(xué)參數(shù)表示的機(jī)器人末端坐標(biāo)系相對于基座坐標(biāo)系的齊次變換矩陣表達(dá)式；

步驟三：根據(jù)輸入的機(jī)器人末端位姿向量，求解對應(yīng)的齊次變換矩陣；

步驟四：根據(jù)末端位置與姿態(tài)的約束條件，判斷輸入的末端位姿是否可達(dá)，若滿足約束條件，則進(jìn)行后續(xù)步驟；否則輸入的末端位姿不可達(dá)，逆解無解；

步驟五：聯(lián)立步驟二所述齊次變換矩陣表達(dá)式和步驟三所述齊次變換矩陣，求解關(guān)節(jié)1的轉(zhuǎn)角θ₁；

步驟六：代入步驟五所述求得的θ₁，對步驟二所述齊次變換矩陣表達(dá)式進(jìn)行分離變量處理，獲得關(guān)于關(guān)節(jié)4的位移d₄的一元二次方程，若方程無實數(shù)解，則輸入的末端位姿不可達(dá)，逆解無解；若方程有實數(shù)解，則根據(jù)期望的機(jī)器人位形，選擇對應(yīng)解作為關(guān)節(jié)4的位移d₄；

步驟七：代入步驟五所述求得的θ₁和步驟六所述求得的d₄，由步驟二所述齊次變換矩陣表達(dá)式中求得關(guān)節(jié)2的轉(zhuǎn)角θ₂的正弦值與余弦值，進(jìn)而求解關(guān)節(jié)2的轉(zhuǎn)角θ₂；

步驟八：聯(lián)立步驟二所述齊次變換矩陣表達(dá)式和步驟三所述齊次變換矩陣，求解關(guān)節(jié)2和關(guān)節(jié)3的轉(zhuǎn)角和θ₂+θ₃；

步驟九：將步驟七所述求得的θ₂代入步驟八所述求得的θ₂+θ₃，求解關(guān)節(jié)3的轉(zhuǎn)角θ₃；

步驟十：判斷求解得到的θ₁、θ₂、θ₃和d₄是否超程，對關(guān)節(jié)超程錯誤進(jìn)行報警，對機(jī)器人各關(guān)節(jié)進(jìn)行行程約束；

末端坐標(biāo)系相對于基座坐標(biāo)系的齊次變換矩陣表達(dá)式為：

其中，c_i表示cosθ_i，s_i表示sinθ_i，s₂₃表示sin(θ₂+θ₃)，c₂₃表示cos(θ₂+θ₃)，a₁、a₂、d₁、d₃、d₄、d₅為常數(shù)；

所述步驟三包括：輸入的機(jī)器人末端位姿向量為：[p_x p_y p_z γ β α]，則該位姿向量對應(yīng)的齊次變換矩陣按下式計算：

p_x、p_y、p_z分別表示機(jī)器人末端在基座坐標(biāo)系下的x、y、z位置坐標(biāo)；γ、β、α分別表示機(jī)器人末端繞基座坐標(biāo)系x、y、z軸的姿態(tài)角；n_x、n_y、n_z分別表示機(jī)器人末端坐標(biāo)系的x軸在基座坐標(biāo)系下的方向余弦；o_x、o_y、o_z分別表示機(jī)器人末端坐標(biāo)系的y軸在基座坐標(biāo)系下的方向余弦；a_x、a_y、a_z分別表示機(jī)器人末端坐標(biāo)系的z軸在基座坐標(biāo)系下的方向余弦；c表示余弦函數(shù)cos()；s表示正弦函數(shù)sin()；

四自由度串聯(lián)機(jī)器人的運動約束為：

輸入的位置和姿態(tài)滿足式(3)的約束條件；

由式(1)和式(2)得到：

因此θ₁直接由下式求解：

θ₁＝Atan2(-o_x,o_y)……(5)

其中，Atan2(x,y)為雙參數(shù)反正切函數(shù)；

關(guān)于d₄的一元二次方程如下：

其中：

K₂＝d₁+d₅a_z-p_z……(8)

若方程無實數(shù)解，則說明輸入的末端位姿不可達(dá)；若有實數(shù)解，則可根據(jù)一元二次方程的求根公式求得d₄的解；

根據(jù)給定的機(jī)器人位形，選取對應(yīng)的解作為d₄的唯一確定解，設(shè)方程的解分別為d′₄和d″₄，若為短臂位形，則：

若為長臂位形，則：

在d₄有實數(shù)解的情況下，繼續(xù)計算θ₂和θ₃的值，如果d₄無解，則退出求解并報錯。