[發(fā)明專利]結(jié)合離線補(bǔ)償和在線跟蹤混合策略的高精度恒力控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202011246946.8
• 申請(qǐng)日: 2020-11-10
• 公開（公告）號(hào): CN112405536B
• 公開（公告）日: 2021-12-28
• 發(fā)明（設(shè)計(jì)）人: 周波;王榮盛;甘亞輝;房芳;何家健;玄甲輝
• 申請(qǐng)（專利權(quán)）人: 東南大學(xué)
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16
• 代理公司: 南京眾聯(lián)專利代理有限公司 32206
• 代理人: 許小莉

1.一種結(jié)合離線補(bǔ)償和在線跟蹤混合策略的高精度恒力控制方法，其特征在于：該方法包括如下步驟：

S1.單元建模：根據(jù)所用機(jī)器人的型號(hào)選取離線編程軟件，然后將接觸式作業(yè)中的工件模型和機(jī)器人模型導(dǎo)入選定的離線編程軟件中，即建立起機(jī)器人運(yùn)動(dòng)學(xué)模型并根據(jù)實(shí)際環(huán)境中工件和機(jī)器人的擺放位置進(jìn)行布局，繼而生成平滑且連續(xù)的運(yùn)動(dòng)參考軌跡序列；

S2.參考軌跡序列的開環(huán)式離線恒力補(bǔ)償：首先建立機(jī)器人關(guān)節(jié)剛度模型，在此基礎(chǔ)上提出基于機(jī)器人關(guān)節(jié)剛度模型的開環(huán)式離線恒力補(bǔ)償方法并通過機(jī)器人關(guān)節(jié)剛度辨識(shí)實(shí)驗(yàn)所求出的機(jī)器人關(guān)節(jié)剛度矩陣K_θ和末端工具手的等效剛度系數(shù)k_t，以及給定的垂直于工件的期望力F_d的一半對(duì)參考軌跡序列中的機(jī)器人末端工具手位姿X_r進(jìn)行開環(huán)式離線恒力補(bǔ)償；然后對(duì)機(jī)器人接觸式作業(yè)中的所有參考軌跡序列點(diǎn)所對(duì)應(yīng)的機(jī)器人末端工具手的位姿X_r進(jìn)行相應(yīng)的開環(huán)式離線恒力補(bǔ)償；

S3.閉環(huán)式在線恒力跟蹤控制的實(shí)現(xiàn)：在對(duì)參考軌跡序列中的機(jī)器人末端工具手位姿X_r實(shí)現(xiàn)開環(huán)式離線恒力補(bǔ)償功能的基礎(chǔ)上，結(jié)合基于力反饋的變剛度自適應(yīng)導(dǎo)納控制方法，即在接觸作業(yè)中，通過力傳感器采集到的來自工件的作用力f與期望作用力f_d的差值作為變剛度自適應(yīng)導(dǎo)納控制器的反饋量來實(shí)現(xiàn)作業(yè)過程中的閉環(huán)式在線恒力跟蹤功能。

2.根據(jù)權(quán)利要求1所述的結(jié)合離線補(bǔ)償和在線跟蹤混合策略的高精度恒力控制方法，其特征在于：步驟S2的具體方法為：

S21.建立機(jī)器人關(guān)節(jié)剛度矩陣K_θ的表達(dá)式：

其中，k_θ1，…,k_θ6分別對(duì)應(yīng)機(jī)器人關(guān)節(jié)1-6的等效彈簧系數(shù)、τ表示關(guān)節(jié)力矩、τ₁，…,τ₆分別對(duì)應(yīng)于機(jī)器人關(guān)節(jié)1-6所受到的力矩、Δθ表示關(guān)節(jié)形變、Δθ₁，…,Δθ₆分別對(duì)應(yīng)機(jī)器人關(guān)節(jié)1-6的形變量；

當(dāng)機(jī)器人工具手末端進(jìn)行接觸式作業(yè)時(shí)，設(shè)機(jī)器人所受的來自工件的擠壓力為F，F(xiàn)＝[F_x，F(xiàn)_y，F(xiàn)_z，T_x，T_y，T_z]，其中F_x，F(xiàn)_y，F(xiàn)_z表示機(jī)械臂的末端工具手在X,Y,Z方向上所受的力，T_x，T_y，T_z表示末端工具手在X,Y,Z方向上所受的力矩，J為機(jī)械臂的雅各比矩陣，J^T為J的轉(zhuǎn)置；

然后可得關(guān)節(jié)力矩τ在工具手進(jìn)行接觸作業(yè)時(shí)的表達(dá)式如下：

τ＝J^TF (2)

由于六自由度串聯(lián)式轉(zhuǎn)動(dòng)關(guān)節(jié)機(jī)械臂在接觸式作業(yè)中通過擠壓工件會(huì)使各個(gè)關(guān)節(jié)發(fā)生形變，每個(gè)關(guān)節(jié)形變經(jīng)過層層傳遞的累計(jì)將導(dǎo)致機(jī)器人工具手末端出現(xiàn)位姿的偏差ΔX，其中ΔX＝[d_x，d_y，d_z，δ_x，δ_y，δ_z]，d_x，d_y，d_z表示工具手末端的位置偏差，δ_x，δ_y，δ_z表示工具手末端的姿態(tài)偏差；

接著可得偏差與各關(guān)節(jié)形變量關(guān)節(jié)如下所示：

ΔX＝JΔθ (3)

最后綜合公式(1)、(2)和(3)可得機(jī)器人關(guān)節(jié)剛度模型：

S22.開環(huán)式離線恒力補(bǔ)償：

在參考軌跡序列中，機(jī)器人工具手末端與工件處于臨界接觸狀態(tài)，機(jī)器人末端工具手的位置和位姿分別用x_r和X_r表示，其中機(jī)器人工具手末端位置和姿態(tài)的期望值分別由和表示；

在機(jī)器人接觸式作業(yè)中，設(shè)機(jī)器人末端工具手與工件環(huán)境接觸時(shí)產(chǎn)生的期望作用力為0.5F_d，由此所導(dǎo)致的機(jī)器人整體的垂直于工件方向上的末端形變量為ΔX_d，首先通過公式(5)計(jì)算得到ΔX_d：

ΔX_d＝ΔX_r+ΔX_t (5)

其中，為機(jī)器人關(guān)節(jié)柔性引起的末端形變量，ΔX_t為末端工具手的形變量；

然后計(jì)算機(jī)器人末端工具手與工件環(huán)境接觸時(shí)產(chǎn)生的期望作用力0.5F_d與末端工具手形變量ΔX_t之間的線性關(guān)系：

其中，k_t為末端工具手等效剛度；

最后由式(5)和(6)可以得到機(jī)械臂的末端工具手經(jīng)過開環(huán)式離線恒力補(bǔ)償后的位姿X_e：

S23.對(duì)機(jī)器人接觸式作業(yè)中的所有參考軌跡序列點(diǎn)所對(duì)應(yīng)的機(jī)器人末端工具手的位姿X_r進(jìn)行相應(yīng)的開環(huán)式離線恒力補(bǔ)償，補(bǔ)償之后的末端工具手的位置和位姿分別是x_e和X_e，以此來通過開環(huán)式的位置補(bǔ)償控制來初步實(shí)現(xiàn)機(jī)器人與工件的接觸式作業(yè)功能。

3.根據(jù)權(quán)利要求1所述的結(jié)合離線補(bǔ)償和在線跟蹤混合策略的高精度恒力控制方法，其特征在于：步驟S3的具體方法為：

S31.在傳統(tǒng)的導(dǎo)納控制算法的基礎(chǔ)上進(jìn)行改進(jìn)得到基于位置的導(dǎo)納控制方法，進(jìn)而得接觸作業(yè)時(shí)機(jī)器人末端工具手與接觸環(huán)境的作用力f和機(jī)器人末端工具手的位置x，具體方法如下：

傳統(tǒng)導(dǎo)納控制算法：

建立一個(gè)二階微分方程來表示機(jī)器人工具手末端的力和位置之間的動(dòng)態(tài)關(guān)系，即導(dǎo)納控制模型：

然后對(duì)上式進(jìn)行拉普拉斯變換,可更直觀看出力與位置之間的轉(zhuǎn)換關(guān)系：

(ms²+bs+k)E(s)＝F(s) (9)

E(s)＝X_d(s)-X_c(s)

其中，m為等效質(zhì)量塊質(zhì)量，b為等效阻尼器的阻尼系數(shù)，k為等效線型彈簧的剛性系數(shù)；

在傳統(tǒng)導(dǎo)納控制的基礎(chǔ)上對(duì)公式(8)中的f用機(jī)器人接觸作業(yè)中的實(shí)際作用力與期望作用力的差值f-f_d來替代；

進(jìn)一步得到改進(jìn)的基于位置的導(dǎo)納控制方法如下：

在接觸式作業(yè)中，設(shè)機(jī)器人的內(nèi)環(huán)中的位置控制器所具的良好跟蹤能力可以保證機(jī)器人的末端執(zhí)行器始終可以進(jìn)行精確的柔順控制，即x＝x_c；設(shè)接觸環(huán)境為線性彈簧模型，其中環(huán)境剛度為k_e，環(huán)境位置為x_e，則得接觸作業(yè)時(shí)機(jī)器人末端工具手與接觸環(huán)境的作用力f和機(jī)器人末端工具手的位置x為：

S32.接觸式作業(yè)中的期望軌跡生成，即將離線補(bǔ)償和在線跟蹤的策略結(jié)合起來實(shí)現(xiàn)機(jī)器人在接觸式作業(yè)中的高精度恒力控制功能：

首先通過公式(11)可得到導(dǎo)納控制器的輸入量，即機(jī)器人末端工具手期望軌跡x_d的表達(dá)式如下：

其中機(jī)器人末端工具手的期望軌跡x_d由環(huán)境位置x_e、期望作用力f_d和環(huán)境剛度k_e所決定，其中x_e已通過前期的離線補(bǔ)償環(huán)節(jié)所獲取，期望作用力f_d是人為給定的常數(shù)，環(huán)境剛度k_e可通過在線估計(jì)獲取，假設(shè)為環(huán)境剛度k_e的估計(jì)值，

然后可得x_d與關(guān)系的表達(dá)式：

最后綜上可得：

由于接觸作業(yè)時(shí)的工件一般是同一種材質(zhì)的物體，故該工件的物體剛度系數(shù)大致為固定數(shù)值，通過離散化系統(tǒng)的數(shù)值積分計(jì)算法可得環(huán)境剛度的估計(jì)值并通過式(12)來對(duì)已經(jīng)經(jīng)過離線補(bǔ)償后的參考軌跡x_e進(jìn)行更新以此來得到期望軌跡x_d；

S33.變剛度自適應(yīng)導(dǎo)納控制算法的實(shí)現(xiàn)，即實(shí)現(xiàn)閉環(huán)式在線恒力跟蹤：

首先提出變剛度導(dǎo)納控制方法，其微分方程如下所示：

其中，自適應(yīng)剛度系數(shù)k(t)的適時(shí)調(diào)整由期望接觸力控制，k_f和k_v分別表示力跟蹤誤差控制的比例系數(shù)與微分系數(shù)；

然后力的跟蹤誤差方程可由上式導(dǎo)出：

在實(shí)現(xiàn)開環(huán)式離線補(bǔ)償功能的基礎(chǔ)上，根據(jù)上述步驟，首先通過在線估計(jì)的方式獲取環(huán)境剛度的估計(jì)值通過式(13)計(jì)算機(jī)器人末端工具手的期望軌跡x_d；然后人為設(shè)定好目標(biāo)剛度自適應(yīng)調(diào)整率中的k_f和k_v；最后根據(jù)自適應(yīng)調(diào)整率計(jì)算變剛度導(dǎo)納控制器中的目標(biāo)剛度系數(shù)k(t)和通過力傳感器采集到的來自工件的作用力f與期望作用力f_d的差值作為變剛度自適應(yīng)導(dǎo)納控制器的反饋量來實(shí)現(xiàn)作業(yè)過程中的閉環(huán)式在線恒力跟蹤功能。