[發(fā)明專利]一種考慮運(yùn)動平穩(wěn)性的冗余機(jī)器人全關(guān)節(jié)避障軌跡優(yōu)化方法

有效

• 申請?zhí)枺?/td>: 201910242408.2
• 申請日: 2019-03-28
• 公開（公告）號: CN109968358B
• 公開（公告）日: 2021-04-09
• 發(fā)明（設(shè)計）人: 劉志峰;許靜靜;裴艷虎;趙永勝
• 申請（專利權(quán)）人: 北京工業(yè)大學(xué)
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 北京思海天達(dá)知識產(chǎn)權(quán)代理有限公司 11203
• 代理人: 沈波

1.一種考慮運(yùn)動平穩(wěn)性的冗余機(jī)器人全關(guān)節(jié)避障軌跡優(yōu)化方法，為冗余自由度關(guān)節(jié)角位移取初值，基于旋量理論建立冗余機(jī)器人逆解模型；

基于以上逆解模型，將機(jī)器人末端在操作空間的目標(biāo)位姿映射到關(guān)節(jié)空間，得到各關(guān)節(jié)對應(yīng)的最終角位移，采用五次多項式在各關(guān)節(jié)的初始角位移與最終角位移間進(jìn)行插值運(yùn)算，得到各關(guān)節(jié)角位移軌跡點(diǎn)列；

依據(jù)機(jī)器人幾何結(jié)構(gòu)，將其等效為由不同直徑的圓柱體組成的整體，以各關(guān)節(jié)角位移軌跡點(diǎn)列為輸入，基于旋量理論計算各圓柱體中心線端點(diǎn)的運(yùn)動路徑點(diǎn)位置；

以各圓柱體中心線端點(diǎn)位置與障礙位置矢量為參量，建立機(jī)器人與障礙間的最小距離模型，用于預(yù)估不同運(yùn)動時刻的避障指標(biāo)；

以冗余關(guān)節(jié)角位移、運(yùn)動時間為變量，同時考慮運(yùn)動學(xué)約束及障礙空間約束，以運(yùn)動時間為優(yōu)化目標(biāo)，確定優(yōu)化問題；

其特征在于：具體實(shí)現(xiàn)步驟如下，

步驟(1)建立冗余機(jī)器人逆解模型；

由于八軸機(jī)器人具有兩個冗余關(guān)節(jié)，為了建立逆解模型，首先為關(guān)節(jié)1和2的角位移θ₁和θ₂取初值，然后基于初始條件，將問題轉(zhuǎn)化為六軸機(jī)器人的逆解問題；

a.求解θ₃、θ₄和θ₅；

取關(guān)節(jié)6、7和8的軸線交點(diǎn)為名義末端，初始位置矢量為q_s＝[L₄ -L₁ -L₃ -L₅]，根據(jù)任務(wù)要求給定末端的目標(biāo)位置矢量為q_e＝[x y z]，那么，基于旋量理論末端繞關(guān)節(jié)3、4和5的運(yùn)動描述如下，

式中表示關(guān)節(jié)i運(yùn)動旋量的指標(biāo)坐標(biāo)形式，由關(guān)節(jié)i的初始單位旋轉(zhuǎn)矢量ω_i及軸上一點(diǎn)的坐標(biāo)q_i計算得到，其取值如式(2)和(3)所示；q_s′和q_e1′分別表示為q_s′＝[q_s 1]^T和其中q_e′＝[q_e 1]^T；

在旋轉(zhuǎn)運(yùn)動中c₁和c₂表示末端在旋轉(zhuǎn)過程中的軌跡交點(diǎn)，根據(jù)三個旋轉(zhuǎn)軌跡的幾何關(guān)系，矢量線段繞軸旋轉(zhuǎn)時，其長度不變；旋轉(zhuǎn)軌跡面垂直于旋轉(zhuǎn)矢量，得到以下方程組，

設(shè)c₁和c₂點(diǎn)所在位置矢量為c₁＝[x₁ y₁ z₁]和c₂＝[x₂ y₂ z₂]，通過聯(lián)立方程組(4)，變量x₁,y₁,z₁,x₂,y₂和z₂求解如下，

式中A、B、A₁和B₁是與運(yùn)動參數(shù)及目標(biāo)位置矢量相關(guān)的常數(shù)，

A＝(L₂+L₃)/L₄；

B＝[x_e1²+(L₁+y_e1)²+(L₂-z_e1)²-L₂²-L₅²+L₄²+L₃²]/(2L₄)；A₁＝A²+1；

B₁＝2(AB-L₂)；C₁＝B²+L₂²-x_e1²-(L₁+y_e1)²-(L₂-z_e1)²；

則關(guān)節(jié)3、4和5的逆解問題轉(zhuǎn)化為3個Paden-Kahan子問題1，

基于子問題1的求解方法，關(guān)節(jié)3、4和5的角位移θ₃、θ₄和θ₅求解如下，

b.求解θ₆和θ₇

關(guān)節(jié)6、7和8的旋轉(zhuǎn)運(yùn)動表示為，

式中

取一點(diǎn)q_s2＝[0 -L₁ -L₃ -L₅]，該點(diǎn)在關(guān)節(jié)軸8上，且在關(guān)節(jié)軸6和7外，因此，繞關(guān)節(jié)6和7旋轉(zhuǎn)至q_e2，q_e2′＝g₁q_s2′，q_e2的運(yùn)動描述如下，

在旋轉(zhuǎn)運(yùn)動中c₃表示點(diǎn)q_s2繞關(guān)節(jié)6和7的旋轉(zhuǎn)軌跡交點(diǎn)，根據(jù)幾何知識得到如下方程組，

通過求解得到軌跡交點(diǎn)坐標(biāo)矢量為

則繞關(guān)節(jié)6和7的運(yùn)動分別表示如下，

同理，基于Paden-Kahan子問題1的求解方法，θ₆和θ₇求解如下，

c.求解θ₈

取關(guān)節(jié)軸8外一點(diǎn)q_s3＝[L₄ 0 -L₃ -L₅]，則該點(diǎn)繞關(guān)節(jié) 軸8的旋轉(zhuǎn)運(yùn)動表示為，

式中

則基于Paden-Kahan子問題1，θ₈表示為，

θ₈＝atan 2[L₁(z_e3+L₃+L₅)，L₁(y_e3+L₁)] (14)

步驟(2)采用五次多項式插值得到關(guān)節(jié)空間軌跡；

通過以上逆解模型，求解與末端目標(biāo)位姿相對應(yīng)的關(guān)節(jié)i的目標(biāo)角位移θ_i；其次，基于五次多項式插值，關(guān)節(jié)i的角位移、角速度及角加速度表示如下，

為保證各關(guān)節(jié)角速度及角加速度的連續(xù)性，且在啟停時關(guān)節(jié)不發(fā)生關(guān)節(jié)變量突變導(dǎo)致的關(guān)節(jié)振顫，得到以下方程組，

式中t_e表示運(yùn)動時間；

通過求解可得參數(shù)a_1,i,a_2,i,a_3,i,a_4,i,a_5,i,a_6,i的值如下，

最后可保證各關(guān)節(jié)運(yùn)動平穩(wěn)性的關(guān)節(jié)角位移表示如下，

θ_i(t)＝6θ_it⁵/t_e⁵-15θ_it⁴/t_e⁴+10θ_it³/t_e³ (18)

步驟(3)計算各圓柱體中心線端點(diǎn)的運(yùn)動路徑點(diǎn)位置；

八軸機(jī)器人可等效為五個臂桿構(gòu)成的機(jī)械結(jié)構(gòu)，將臂桿i等效為半徑為R_i(mm)的圓柱體，則根據(jù)各圓柱體中心線端點(diǎn)的初始位置，基于旋量理論其在t時刻的位置坐標(biāo)計算如下，

其中J_i(t)和J_i+1(t)分別表示第i個圓柱體中心線端點(diǎn)的當(dāng)前位置坐標(biāo)，1≤i≤5；步驟(4)建立機(jī)器人與障礙間的最小距離模型；

避開球形障礙，半徑均設(shè)為R_o(mm)，中心位置坐標(biāo)為O＝[x_o y_o z_o]，取障礙中心O在臂桿i中心線段J_iJ_i+1上垂足為O′，取步驟(3)中計算得到的中心線端點(diǎn)J_i與J_i+1的當(dāng)前位置坐標(biāo)分別為J_i＝[x_J,i y_J,i z_J,i]和J_i+1＝[x_J,i+1 y_J,i+1 z_J,i+1]，則直線J_iJ_i+1的參數(shù)方程表示如下，

式中參數(shù)0≤η≤1表示點(diǎn)在中心線段J_iJ_i+1上，否則，點(diǎn)在中心線段外；

設(shè)垂足O′的坐標(biāo)位置為O′＝[x_o′ y_o′ z_o′]，通過以下方程求得，

當(dāng)垂足落在中心線段J_iJ_i+1上，設(shè)O′＝[x_o′ y_o′ z_o′]，此時臂桿i與障礙間的最小距離表示為，

當(dāng)垂足未落在中心線段J_iJ_i+1上，臂桿i與障礙間的最小距離表示為，

則機(jī)器人與障礙間的最小距離表示為，

d＝min[d_i|1≤i≤5] (24)

步驟(5)優(yōu)化問題描述；

在避障軌跡規(guī)劃問題中，以冗余關(guān)節(jié)角位移θ₁，θ₂和運(yùn)動時間t_e為優(yōu)化變量，同時考慮運(yùn)動學(xué)約束及障礙空間約束即機(jī)器人與障礙間的最小距離d不應(yīng)小于任務(wù)所要求的安全閾值d_t，以最小化運(yùn)動時間為優(yōu)化目標(biāo)，則優(yōu)化問題可描述如下，

f＝min{t_e}

式中θ_i,min，θ_i,max為各關(guān)節(jié)角位移極限值，和分別表示各關(guān)節(jié)角速度、角加速度的最大允許值，1≤i≤8；

步驟(6)優(yōu)化軌跡；

設(shè)定初始條件，采用粒子群算法搜索最優(yōu)軌跡，以保證運(yùn)動性能。