本發(fā)明涉及一種用于單自由度柔性末端的機械臂控制算法，屬于機械臂結(jié)構(gòu)和控制技術(shù)領(lǐng)域。

背景技術(shù)：

機械臂被廣泛使用在機械制造、航空航天、醫(yī)藥、原子能等領(lǐng)域，對自動化生產(chǎn)起到了重大作用。機械臂在運動中可以分為自由運動和受力運動，自由運動一般應(yīng)用在噴漆、焊接、碼垛等工作中，在這樣的環(huán)境中機械臂不直接和外界環(huán)境接觸，所以在控制上只需要考慮路徑規(guī)劃問題，目前該方面的研究應(yīng)用較為成熟。對于受力運動即機械臂在運動的過程中和環(huán)境發(fā)生交互作用，需要和外界進行接觸來完成指定的功能，像打磨、鉆孔、拋光等，最簡單的莫過于抓取，在受力運動中，機械臂不但要進行精確的位置控制，而且需恰當(dāng)?shù)乜刂平佑|力大小，否則難以完成任務(wù)甚至損壞器物。這種能對外界力做出運動調(diào)整的控制方式稱為柔順控制，極大地拓展了機械臂的安全性與應(yīng)用領(lǐng)域。

機械臂與外界環(huán)境接觸力的控制體現(xiàn)出機械臂具有一定的柔順性。為了使機器人對外界具有柔順性，現(xiàn)有的控制方法可以分為兩種：被動柔順控制和主動柔順控制。被動柔順即利用能夠儲存或吸收能量的機械器件把機械臂與作用環(huán)境分隔開，如彈簧、阻尼等具有彈性的機構(gòu)，使機器人與環(huán)境相接觸時對外部的作用力產(chǎn)生一定的自然順從能力。最具有代表性的被動柔順裝置是美國一個實驗室設(shè)計的一種稱為遠程中心柔順的無源機械裝置，它是一個由六根彈簧構(gòu)成的能順從空間六個自由度的柔順手腕，但它不能使機器人本身產(chǎn)生對力的反應(yīng)。另外被動柔順控制由于彈性構(gòu)件的變形導(dǎo)致難以精確控制位置，因而其一般應(yīng)用在要求比較低的場合。

主動柔順控制是把力完整的體現(xiàn)在控制系統(tǒng)中，應(yīng)用控制算法來抵消位置和力雙向誤差，如阻抗控制、剛度控制、力/位混合控制等，但往往需要機械臂開放力矩控制，這是目前市場上的機械臂難以做到的，而且力矩控制需要從底層控制器上進行算法編程，大大地增加了控制算法的復(fù)雜程度。

技術(shù)實現(xiàn)要素：

有鑒于此，本發(fā)明提供了一種用于單自由度柔性末端的機械臂控制算法，通過速度控制的方式來實現(xiàn)機械臂和環(huán)境之間的力交互控制，簡化了算法的復(fù)雜性；通過在機械臂末端安裝力傳感器的方式對接觸力進行測量，從而完成主動柔順控制；通過在工具和力傳感器之間增加單自由度的柔性連接來提高位置控制精度，即結(jié)合了傳統(tǒng)的被動柔順控制和主動柔順控制的優(yōu)點，從而使機械臂完成與環(huán)境之間的良好交互控制。

一種用于單自由度柔性末端的機械臂控制算法，該算法基于單自由度柔性末端的機械臂，機械臂末端固定連接力傳感器，力傳感器與軸承套固定連接，工具通過直線軸承安裝在軸承套中，工具只能沿直線軸承的軸向滑動，彈簧安裝在軸承套內(nèi)部的空腔內(nèi)，彈簧的兩端分別與工具和力傳感器抵觸；

該算法首先建立世界坐標(biāo)系、接觸力測量坐標(biāo)系和運動坐標(biāo)系，通過柔順度來建立起機械臂末端運動速度和接觸力的關(guān)系，整體的柔順度為機械臂的柔順度、工具末端的柔順度以及環(huán)境的柔順度之和；由所需控制的接觸力得到機械臂末端運動速度，通過逆運動學(xué)求解完成機械臂各個關(guān)節(jié)角的轉(zhuǎn)動速度并控制機械臂運動，再通過pd控制器使接觸力達到穩(wěn)定值。

進一步地，所述控制算法的計算步驟如下：

第一步：建立坐標(biāo)系

機械臂的基座上建立世界坐標(biāo)系{w}，在力傳感器測量端面上建立接觸力測量坐標(biāo)系{s}，此外，再建立一個運動坐標(biāo)系{c}，運動坐標(biāo)系{c}以工具末端和環(huán)境的接觸點為原點，工具的軸線方向為z軸；

第二步：求解機械臂末端與環(huán)境接觸點在世界坐標(biāo)系{w}下的速度

機械臂末端與環(huán)境的接觸點在世界坐標(biāo)系{w}中的輸出位置為y，通過運動學(xué)關(guān)系可以表示為：

y＝f(q,xu)(1)

其中q為機械臂的關(guān)節(jié)角，xu為整個系統(tǒng)的未能控制的自由度所產(chǎn)生的位移，包括環(huán)境接觸的變形量以及彈簧的變形量；交互環(huán)境限定為剛性環(huán)境，環(huán)境的變形量可以忽略不計，因此整個系統(tǒng)未能控制的自由度所產(chǎn)生的位移即為彈簧的變形量δx：

δx＝fs/ks(2)

其中ks為彈簧的剛度，fs為施加在彈簧上的力由傳感器測量得到，由于工具僅沿著{s}坐標(biāo)系的z軸方向運動，因此坐標(biāo)系{c}相對于{s}的齊次變換矩陣為：

其中l(wèi)為彈簧不受力時工具末端到坐標(biāo)系{s}原點的距離，因此從末端接觸點到世界坐標(biāo)的正運動學(xué)變換可以表示為：

其中為坐標(biāo)系{s}相對于世界坐標(biāo)系{w}的變換矩陣；在基于速度的控制方法中，計算目標(biāo)速度的大?。?/p>

其中

ks＝diag(0,0,-1/ks,0,0,0)(6)

jq為機械臂的雅可比矩陣，將關(guān)節(jié)速度映射到世界坐標(biāo)系下的笛卡爾速度；為從坐標(biāo)系{s2}到{w}的速度轉(zhuǎn)換矩陣；

第三步：求解機械臂在世界坐標(biāo)系{w}下的柔順度

在使用基于速度控制的方法時，力不能被直接控制，必須轉(zhuǎn)化成位移，因此力和速度的關(guān)系表示為：

其中cy為在世界坐標(biāo)系下的柔順度之和，f為施加在機械臂末端上的力，為機械臂末端輸出的速度；

為了控制具有一定柔順性的機械臂，只需要考慮靜力學(xué)，從而機械臂的驅(qū)動模型可以表示為：

τq＝kq(qd-q)(8)

其中τq為關(guān)節(jié)輸出力矩，kq為控制系統(tǒng)的增益矩陣，q為關(guān)節(jié)角，即關(guān)節(jié)力矩的大小和關(guān)節(jié)的運動速度成正比；另外，機械臂的關(guān)節(jié)空間和末端輸出的速度以及力的轉(zhuǎn)換關(guān)系有：

jq^tf＝τq(9)

將式(9)、(10)代入式(8)整理可得到：

對比式(7)可以得到機械臂在世界坐標(biāo)系{w}下的柔順度為：

cr＝j(luò)qkq^-1jq^t(12)

第四步，求解工具末端以及環(huán)境在世界坐標(biāo)系{w}下的柔順度

為了計算出整體的柔順度，在此還需要考慮彈簧的變形，由于限定環(huán)境為剛性，因此，環(huán)境的柔順度為0，工具和環(huán)境的接觸為點接觸，從而可以得到接觸力f和彈簧所受到的力fs之間的關(guān)系為：

fs＝fcos(θ)(13)

其中θ為接觸力和工具軸線方向的夾角，將式(12)和(13)帶入式(5)可以得到末端接觸點的速度在世界坐標(biāo)系下為：

因為控制的目標(biāo)是為了給環(huán)境施加一個靜態(tài)的力，即代入式(14)，

可得

由于整體的柔順度為機械臂和工具的柔順度之和，因此，工具的柔順度表示為：

第五步：通過pd控制器使接觸力達到穩(wěn)定值

通過柔順度的計算從而可得到力和速度的關(guān)系，另外為了控制接觸力能達到一個穩(wěn)定的值，在此采用pd控制器，傳統(tǒng)的pd控制器為：

其中為控制輸出量(速度)，e為控制輸入量和反饋量的差，kp,kd∈r^6×6為對角矩陣，分別為比例控制系數(shù)和微分控制系數(shù)；當(dāng)控制的輸入量為力時，可通過式(7)建立起速度和力的關(guān)系，結(jié)合pd控制器，從而可以得到：

其中fc為工具和環(huán)境之間的接觸力反饋，fc_d為需要達到的接觸力；由于反饋的線性化，因此可以找到合適的矩陣來使整個閉環(huán)系統(tǒng)達到穩(wěn)定的狀態(tài)。

有益效果：

1、本發(fā)明克服了現(xiàn)有柔順控制存在的一些缺點，通過速度控制的方式來實現(xiàn)機械臂和環(huán)境之間的力交互控制，簡化了算法的復(fù)雜性，降低了對機械臂的硬件配置要求，有利于人機協(xié)作機械臂的推廣。

2、本發(fā)明結(jié)合了傳統(tǒng)的被動柔順控制和主動柔順控制的優(yōu)點，通過主動柔順控制的方式補償了被動柔順機構(gòu)位置誤差大的問題，從而使機械臂完成與環(huán)境之間的良好力交互控制。

附圖說明

圖1為本發(fā)明機械臂末端的結(jié)構(gòu)示意圖；

圖2為本發(fā)明機械臂控制場景示意圖。

其中，1-機械臂末端、2-力傳感器、3-直線軸承、4-工具、5-彈簧、6-軸承套。

具體實施方式

下面結(jié)合附圖并舉實施例，對本發(fā)明進行詳細(xì)描述。

本發(fā)明提供了一種用于單自由度柔性末端的機械臂控制算法，該算法基于單自由度柔性末端的機械臂，如附圖1所示，機械臂末端1固定連接力傳感器2，力傳感器2與軸承套6固定連接，工具4通過直線軸承3安裝在軸承套6中，工具4只能沿直線軸承3的軸向滑動，彈簧5安裝在軸承套6內(nèi)部的空腔內(nèi)，彈簧5的兩端分別與工具4和力傳感器2抵觸。

機械臂的控制場景如附圖2所示，機械臂的基座上建立世界坐標(biāo)系{w}，在力傳感器測量端面上建立接觸力測量坐標(biāo)系{s}，此外，再建立一個運動坐標(biāo)系{c}，運動坐標(biāo)系{c}以工具末端和環(huán)境的接觸點為原點，工具的軸線方向為z軸。

通過整體柔順度來建立起機械臂末端運動速度和接觸力的關(guān)系，整體的柔順度cy為機械臂的柔順度cr，工具末端的柔順度ct以及環(huán)境的柔順度ce之和，由所需控制的接觸力f得到機械臂末端運動速度通過逆運動學(xué)求解完成機械臂各個關(guān)節(jié)角的轉(zhuǎn)動速度并控制機械臂運動，再通過pd控制器使接觸力達到穩(wěn)定值。

本發(fā)明的控制算法的計算步驟如下：

第一步：建立坐標(biāo)系

機械臂的基座上建立世界坐標(biāo)系{w}，在力傳感器測量端面上建立接觸力測量坐標(biāo)系{s}，此外，再建立一個運動坐標(biāo)系{c}，運動坐標(biāo)系{c}以工具末端和環(huán)境的接觸點為原點，工具的軸線方向為z軸；

第二步：求解機械臂末端與環(huán)境接觸點在世界坐標(biāo)系{w}下的速度

機械臂末端與環(huán)境的接觸點在世界坐標(biāo)系{w}中的輸出位置為y，通過運動學(xué)關(guān)系可以表示為：

y＝f(q,xu)(1)

其中q為機械臂的關(guān)節(jié)角，xu為整個系統(tǒng)的未能控制的自由度所產(chǎn)生的位移，包括環(huán)境接觸的變形量以及彈簧的變形量；交互環(huán)境限定為剛性環(huán)境，環(huán)境的變形量可以忽略不計，因此整個系統(tǒng)未能控制的自由度所產(chǎn)生的位移即為彈簧的變形量δx：

δx＝fs/ks(2)

其中ks為彈簧的剛度，fs為施加在彈簧上的力由傳感器測量得到，由于工具僅沿著{s}坐標(biāo)系的z軸方向運動，因此坐標(biāo)系{c}相對于{s}的齊次變換矩陣為：

其中l(wèi)為彈簧不受力時工具末端到坐標(biāo)系{s}原點的距離，因此從末端接觸點到世界坐標(biāo)的正運動學(xué)變換可以表示為：

其中為坐標(biāo)系{s}相對于世界坐標(biāo)系{w}的變換矩陣；在基于速度的控制方法中，計算目標(biāo)速度的大小：

其中

ks＝diag(0,0,-1/ks,0,0,0)(6)

jq為機械臂的雅可比矩陣，將關(guān)節(jié)速度映射到世界坐標(biāo)系下的笛卡爾速度；為從坐標(biāo)系{s2}到{w}的速度轉(zhuǎn)換矩陣；

第三步：求解機械臂在世界坐標(biāo)系{w}下的柔順度

在使用基于速度控制的方法時，力不能被直接控制，必須轉(zhuǎn)化成位移，因此力和速度的關(guān)系表示為：

其中cy為在世界坐標(biāo)系下的柔順度之和，f為施加在機械臂末端上的力，為機械臂末端輸出的速度；

為了控制具有一定柔順性的機械臂，只需要考慮靜力學(xué)，從而機械臂的驅(qū)動模型可以表示為：

τq＝kq(qd-q)(8)

其中τq為關(guān)節(jié)輸出力矩，kq為控制系統(tǒng)的增益矩陣，q為關(guān)節(jié)角，即關(guān)節(jié)力矩的大小和關(guān)節(jié)的運動速度成正比；另外，機械臂的關(guān)節(jié)空間和末端輸出的速度以及力的轉(zhuǎn)換關(guān)系有：

jq^tf＝τq(9)

將式(9)、(10)代入式(8)整理可得到：

對比式(7)可以得到機械臂在世界坐標(biāo)系{w}下的柔順度為：

cr＝j(luò)qkq^-1jq^t(12)

第四步，求解工具末端以及環(huán)境在世界坐標(biāo)系{w}下的柔順度

為了計算出整體的柔順度，在此還需要考慮彈簧的變形，由于限定環(huán)境為剛性，因此，環(huán)境的柔順度為0，工具和環(huán)境的接觸為點接觸，從而可以得到接觸力f和彈簧所受到的力fs之間的關(guān)系為：

fs＝fcos(θ)(13)

其中θ為接觸力和工具軸線方向的夾角，將式(12)和(13)帶入式(5)可以得到末端接觸點的速度在世界坐標(biāo)系下為：

因為控制的目標(biāo)是為了給環(huán)境施加一個靜態(tài)的力，即代入式(14)，

可得

由于整體的柔順度為機械臂和工具的柔順度之和，因此，工具的柔順度表示為：

第五步：通過pd控制器使接觸力達到穩(wěn)定值

通過柔順度的計算從而可得到力和速度的關(guān)系，另外為了控制接觸力能達到一個穩(wěn)定的值，在此采用pd控制器，傳統(tǒng)的pd控制器為：

其中為控制輸出量(速度)，e為控制輸入量和反饋量的差，kp,kd∈r^6×6為對角矩陣，分別為比例控制系數(shù)和微分控制系數(shù)；當(dāng)控制的輸入量為力時，可通過式(7)建立起速度和力的關(guān)系，結(jié)合pd控制器，從而可以得到：

其中fc為工具和環(huán)境之間的接觸力反饋，fc_d為需要達到的接觸力；由于反饋的線性化，因此可以找到合適的矩陣來使整個閉環(huán)系統(tǒng)達到穩(wěn)定的狀態(tài)。

綜上所述，以上僅為本發(fā)明的較佳實施例而已，并非用于限定本發(fā)明的保護范圍。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進等，均應(yīng)包含在本發(fā)明的保護范圍之內(nèi)。