本發(fā)明屬于機器人控制領域，主要涉及一種半柔性機械臂系統(tǒng)的控制方法。

背景技術：

隨著上世紀60年代第一臺工業(yè)機器人問世以來，機器人的應用已逐漸滲透到航天、醫(yī)療、軍事，甚至日常生活及娛樂教育等各個領域。起初制造機器人更多是把它作為一種自動化裝置，服務于制造業(yè)。隨著理論和技術的日益成熟，人們對機器人提出了越來越高的要求，越來越多的場合需要機器人或者機械臂為我們服務。

機械臂是機器人最主要的執(zhí)行機構。在當下熱門的無人車上加入機械臂可以協(xié)助或者幫助人類完成許多任務，比如自動加油、自動清障等。通常機械臂的工作空間在設計時可能無法滿足復雜多變的環(huán)境，因此有必要給機械臂配備工具，以提高它的工作范圍，進而滿足實際需求。

機械臂的主體是剛性的，但附加的工具可能是帶有柔性的連桿(有一定的柔性更符合實際情況)。當工具末端帶有負載(如汽油槍、帶清理的障礙物等)，柔性連桿末端在進行運動時會出現(xiàn)變形。在對機械臂進行控制時，比如軌跡跟蹤，柔性變形會對運動學和動力學模型產(chǎn)生很大影響。簡單地把工具的影響當成擾動不能很好地解決問題，而是需要把工具當成整個機器人系統(tǒng)的一部分。這樣就擴展了本身的機械臂，組成了一個剛性主體、柔性附件的半柔性機械臂系統(tǒng)。由于工具是柔性的，所以需要應用柔性機械臂中柔性連桿或者柔性關節(jié)的動力學建模的相關方法。半柔性機械臂和柔性機械臂是都是一個剛?cè)狁詈系姆蔷€性系統(tǒng)，系統(tǒng)的動力學特性和控制特性相互耦合。

柔性連桿實際上一個無窮多自由度的柔性系統(tǒng)，它的建模方法主要有假設模態(tài)法、有限元法、集總參數(shù)法等。但是這些方法的特點都是結合彈性力學，建立一系列的偏微分方程去描述彈性變形，然后按照拉格朗日或者牛頓-歐拉法的思路去建立柔性機械臂的動力學模型，然而這種建模和控制過程很復雜，對于半柔性機械臂來說，其主體是剛性的，僅針對其中一個柔性連桿采用如此復雜的建模和控制，是沒有必要的，也不能滿足實際需求。

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

有鑒于此，本發(fā)明提供了一種半柔性機械臂系統(tǒng)的控制方法，針對半柔性機械臂系統(tǒng)，把連桿柔性變形建模成隨機干擾，利用遞推牛頓-歐拉動力學算法建立半柔性機械臂的動力學模型，針對所建立的動力學模型采用雙閉環(huán)運動控制方法進行運動控制，能夠適應半柔性機械臂的控制需求。

為了達到上述目的，本發(fā)明的技術方案為：一種半柔性機械臂系統(tǒng)的控制方法，半柔性機械臂系統(tǒng)由剛性機械臂和附加的柔性連桿組成，剛性機械臂是由一系列的剛性連桿和關節(jié)構成，其中關節(jié)為轉(zhuǎn)動關節(jié)或者移動關節(jié)；方法包括如下步驟：

步驟1、剛性機械臂共有n+1個剛性連桿，編號從A₀到A_n，相鄰剛性連桿之間通過關節(jié)連接，共有n個關節(jié)，則關節(jié)編號從B₁到B_n；附加的柔性連桿為R，R與剛性連桿A_n通過關節(jié)B_n+1連接；

步驟2、剛性機械臂采用改進型的DH參數(shù)方法建立得到{0}，{1}，{2}，…，{n-1}，{n}共n+1個剛性連桿的固連坐標系，并計算得到n+1個剛性連桿的連桿參數(shù)；附加的柔性連桿為R的三個坐標軸分別規(guī)定為：采用改進型的DH參數(shù)方法定義關節(jié)B_n的軸線作為Z軸，然后把關節(jié)B_n+1到柔性連桿R的末端的連線所在直線作為X軸，再根據(jù)右手坐標系的原則確定Y軸；

步驟3、剛性機械臂按照原有的連桿參數(shù)規(guī)定不變；對于柔性連桿R，將柔性連桿R視作剛性連桿，進行連桿參數(shù)的計算，其中連桿參數(shù)中的柔性連桿R的長度為從關節(jié)B_n+1到柔性連桿R的末端的線段長度L_n+1，L_n+1為可變值，其他參數(shù)不變；

步驟4、根據(jù)連桿參數(shù)確定剛性連桿A₀到A_n以及柔性連桿R之間相鄰連桿的坐標系變換公式；

步驟5、用遞推牛頓-歐拉動力學算法建立半柔性機械臂系統(tǒng)的動力學方程，即建立關節(jié)力矩與關節(jié)角、關節(jié)速度、關節(jié)加速度之間的關系；

步驟6、在上述動力學方程的基礎上，依據(jù)如下方法對半柔性機械臂系統(tǒng)進行控制：

step601、先假設半柔性機械臂系統(tǒng)中連桿均為剛性連桿，根據(jù)預先給出的笛卡爾空間中期望的運動軌跡規(guī)劃出關節(jié)空間中的期望軌跡，用關節(jié)角度qd、速度和加速度表達；

step602、根據(jù)step601中計算得到的關節(jié)空間中的期望軌跡采用魯棒自適應PD控制算法，計算得到關節(jié)力矩τ為

其中前兩項為PD控制部分，e和分別表示關節(jié)角度和關節(jié)角速度的跟蹤誤差，K_p和K_v分別是PD控制中的位置和速度反饋對應的比例系數(shù)矩陣，為補償動力學模型的自適應控制項，t_d表示補償干擾的魯棒控制項；

step603、將step602控制后的關節(jié)空間的實際軌跡映射到笛卡爾空間，將實際軌跡的笛卡爾空間映射值與笛卡爾空間中期望的運動軌跡進行誤差計算，并依據(jù)計算得到的誤差為規(guī)劃出關節(jié)空間中的期望軌跡增加一個調(diào)整值，返回step602，并重復step602和step603，直至誤差滿足精度要求。

進一步地，步驟5中，關節(jié)力矩表示為τ，q＝[θ₁ … θ_n θ_n+1]^T表示前n個關節(jié)的角度θ₁～θ_n和第n+1個關節(jié)的角度測量值θ_n+1組成的關節(jié)向量，其中θ_n+1為柔性連桿R變形前測得的關節(jié)角度；表示前n個關節(jié)的角度θ₁～θ_n和虛擬關節(jié)角度組成的關節(jié)角向量，為柔性連桿變形后的虛擬關節(jié)角度；則半柔性機械臂系統(tǒng)的動力學方程具體如下：

其中，為關節(jié)力矩，M⁰為機械臂變形前相應的慣性矩陣、C⁰為機械臂變形前相應的離心力和哥氏力矩陣、G⁰為機械臂變形前相應的重力項，和分別為q的二階和一階導數(shù)，ω為擾動，ΔC為機械臂變形前相應的離心力和哥氏力矩陣變化量，ΔG為機械臂變形前后相應的重力項變化量，為關節(jié)角加速度向量變化量，為關節(jié)角向量速度變化量。

進一步地，step603中以笛卡爾空間的軌跡誤差的平方和作為目標函數(shù)R，關節(jié)空間的期望節(jié)軌跡作為控制變量，求出目標函數(shù)R對控制變量的各個分量的負梯度方向分別為則調(diào)整值為：其中μ為設定的學習步長。

有益效果：

本發(fā)明提供的控制方法，針對半柔性機械臂系統(tǒng)，在合理假設的前提下，在連桿附加坐標系特殊規(guī)定的基礎上建立半柔性機械臂的運動學模型，把連桿柔性變形建模成隨機干擾，利用遞推牛頓-歐拉動力學算法建立半柔性機械臂的動力學模型，針對所建立的動力學模型采用雙閉環(huán)運動控制方法，其中以step602～step603作為外環(huán)，采用學習控制，通過期望的位置坐標，這是整個半柔性機械臂系統(tǒng)需要實現(xiàn)的跟蹤目標，得到期望的關節(jié)軌跡；以step602作為內(nèi)環(huán)，采用關節(jié)軌跡控制，通過外環(huán)的控制量即期望的關節(jié)軌跡，計算得到關節(jié)力矩。本發(fā)明方法針對主體為剛性，具有一個柔性連桿的半柔性機械臂，適應性強、效果好。

附圖說明

圖1是由剛性機械臂和柔性連桿組成的半柔性機械臂系統(tǒng)圖，代表一種半柔性機械臂系統(tǒng)的通用結構；

圖2是一種2自由度半柔性機械臂的系統(tǒng)簡圖。2自由度半柔性機械臂系統(tǒng)位于豎直平面內(nèi)，其中剛性主體部分含有1個關節(jié)，基座連桿省略不畫，因此共標出一個剛性連桿加上一個柔性連桿，共2個自由度；

圖3是圖2所示的2自由度半柔性機械臂的運動學模型示意圖。其中標明了機械臂的參數(shù)；

圖4是半柔性機械臂的運動控制系統(tǒng)框圖；

圖5是半柔性機械臂的運動控制流程圖。

具體實施方式

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

一種半柔性機械臂系統(tǒng)的控制方法，半柔性機械臂系統(tǒng)由剛性機械臂和附加的柔性連桿組成，剛性機械臂是由一系列的剛性連桿和轉(zhuǎn)動關節(jié)或者移動關節(jié)構成；建模方法包括如下步驟：

步驟1、剛性機械臂共有n+1個剛性連桿，編號從A₀到A_n，相鄰剛性連桿之間通過關節(jié)連接，共有n個關節(jié)，則關節(jié)編號從B₁到B_n；附加的柔性連桿為R，R與A_n通過關節(jié)B_n+1連接；

步驟2、剛性機械臂采用改進型的DH參數(shù)方法建立得到{0}，{1}，{2}，…，{n-1}，{n}共n+1個剛性連桿的固連坐標系，并計算得到n+1個剛性連桿的連桿參數(shù)；附加的柔性連桿為R的三個坐標軸分別規(guī)定為：采用改進型的DH參數(shù)方法定義關節(jié)B_n的軸線作為Z軸，然后把關節(jié)B_n+1到柔性連桿R的末端的連線所在直線作為X軸，再根據(jù)右手坐標系的原則確定Y軸，

步驟3、剛性機械臂按照原有的連桿參數(shù)規(guī)定不變；對于柔性連桿R，將柔性連桿R視作剛性連桿，進行連桿參數(shù)的計算，其中連桿參數(shù)中的柔性連桿R的長度為從關節(jié)B_n+1到柔性連桿R的末端的線段長度L_n+1，L_n+1為可變值，其他參數(shù)不變；

L_n+1＝l_n+1+Δl_n+1

步驟4、根據(jù)連桿參數(shù)確定剛性連桿A₀到A_n以及柔性連桿R之間相鄰連桿的坐標系變換公式；

步驟5、用遞推牛頓-歐拉動力學算法建立半柔性機械臂系統(tǒng)的動力學方程，即建立關節(jié)力矩與關節(jié)角、關節(jié)速度和關節(jié)加速度之間的關系；關節(jié)力矩表示為τ，q＝[θ₁ … θ_nθ_n+1]^T表示前n個關節(jié)的角度θ₁～θ_n和第n+1個關節(jié)的角度測量值θ_n+1組成的關節(jié)向量，其中θ_n+1為柔性連桿R變形前測得的關節(jié)角度；表示前n個關節(jié)的角度θ₁～θ_n和虛擬關節(jié)角度組成的關節(jié)角向量，為柔性連桿變形后的虛擬關節(jié)角度；則半柔性機械臂系統(tǒng)的動力學方程具體如下：

其中，τ為關節(jié)力矩，M⁰為機械臂變形前相應的慣性矩陣、C⁰為機械臂變形前相應的離心力和哥氏力矩陣、G⁰為機械臂變形前相應的重力項，和分別為q的二階和一階導數(shù)，ω為擾動，ΔM為機械臂變形前后相應的慣性矩陣變化量，ΔC為機械臂變形前后相應的離心力和哥氏力矩陣變化量，ΔG為機械臂變形前后相應的重力項變化量，為關節(jié)角向量變化量，為關節(jié)加速度向量變化量，為關節(jié)速度向量變化量。

步驟6、在上述動力學方程的基礎上，依據(jù)如下方法對半柔性機械臂系統(tǒng)進行控制：

step601、先假設半柔性機械臂系統(tǒng)中連桿均為剛性連桿，根據(jù)預先給出的笛卡爾空間中期望的運動軌跡規(guī)劃出關節(jié)空間中的期望軌跡，用關節(jié)角度q_d、速度和加速度表達；

step602、根據(jù)step601中計算得到的關節(jié)空間中的期望軌跡采用魯棒自適應PD控制對半柔性機械臂系統(tǒng)實施控制，獲得關節(jié)力矩τ，依據(jù)τ對半柔性機械臂系統(tǒng)進行控制；

用q_d和分別表示期望關節(jié)角和期望關節(jié)角速度。內(nèi)環(huán)的關節(jié)空間控制器輸出的控制量為關節(jié)力矩，采用魯棒自適應PD控制算法，總表達式為

其中前兩項為PD控制部分，e,分別表示關節(jié)角度和關節(jié)角速度的跟蹤誤差，K_p和K_v分別是PD控制中的位置和速度反饋對應的比例系數(shù)矩陣，為補償動力學模型的自適應控制項，t_d表示補償干擾的魯棒控制項。

step603、將step602控制后的關節(jié)空間的實際軌跡映射到笛卡爾空間，將實際軌跡的笛卡爾空間映射值與笛卡爾空間中期望的運動軌跡進行誤差計算，并依據(jù)計算得到的誤差為規(guī)劃出關節(jié)空間中的期望軌跡增加一個調(diào)整值，返回step602，直至誤差滿足精度要求。

步驟6中以笛卡爾空間的軌跡誤差的平方和作為目標函數(shù)，關節(jié)空間的期望節(jié)軌跡作為控制變量，求出目標函數(shù)R對控制變量的各個分量的負梯度方向分別為則調(diào)整值為：其中μ為設定的學習步長。

實施例、

圖1是由剛性機械臂和柔性連桿組成的半柔性機械臂系統(tǒng)圖，代表一種通用的結構。

本發(fā)明提供的一種半柔性機械臂的運動學和動力學建模方法，具體實施步驟如下：

步驟1.連桿和關節(jié)編號的規(guī)定。如圖2所示，2自由度半柔性機械臂共有兩個剛性連桿，O₀基座定義為第0號連桿(圖2中省略)，O₁到O₂的連線是第1號剛性連桿，O₁表示第1個關節(jié)，O₂表示第2個關節(jié)，是第2號連桿，是柔性的。這樣圖2所示的半柔性機械臂系統(tǒng)共有2個自由度。

步驟2.對連桿附加坐標系的規(guī)定。為了建立運動學模型，需要給出半柔性機械臂系統(tǒng)連桿附加坐標系的規(guī)定。如圖3所示，剛性主體部分采用改進型的DH參數(shù)方法:和O₀Y₀分別為基座連桿的坐標系{0}的X軸和Y軸，Z₀軸為過O₀的直線，且正方向指向紙面外；和O₁Y₁分別為第1個連桿的固連坐標系{1}的X軸和Y軸，Z₁軸為過O₁的直線，且正方向指向紙面外；附加在末端的柔性連桿的坐標系{2}的坐標軸分別規(guī)定為：Z₂定義為O₂的直線，且正方向指向紙外；把關節(jié)原點O₂到末端B的連線所在直線規(guī)定為X軸，這樣就類似于定義了一個虛擬的連桿；再根據(jù)右手坐標系的原則,即由X軸逆時針旋轉(zhuǎn)90度得到Y軸。由于整個機械臂系統(tǒng)分布在豎直平面，且僅有兩個自由度，因此Z軸在圖3中被省略掉了。

步驟3.連桿參數(shù)的規(guī)定。剛性主體部分按照原有的連桿參數(shù)規(guī)定不變。如圖3所示，連桿參數(shù)的規(guī)定如下：第1個剛性連桿的長度為l₁，質(zhì)量為m₁，關節(jié)角度為θ₁；第2個柔性連桿，原長為l₂，定義虛擬連桿的長度為L₂，由于是柔性連桿，可認為連桿質(zhì)量和負載質(zhì)量相比可以忽略，這樣質(zhì)量幾乎集中在末端，記為m₂，可測的關節(jié)角度為θ₂，虛擬關節(jié)角為無法直接測得。根據(jù)定義得到下面的表達式，

L₂＝l₂+Δl₂

其中，Δl₂和Δθ₂反映了柔性變形導致的偏差。

步驟4.運動學模型的建立。對連桿附加坐標系和連桿參數(shù)的規(guī)定考慮了柔性變形的影響，把帶有柔性連桿的機械臂等效成了一種特殊的剛性機械臂。相鄰連桿坐標系{0}和{1}、{1}和{2}的變換公式由下面兩個式子給出：

這樣根據(jù)連桿坐標系和相應的連桿參數(shù)，就可以按照一般剛性機械臂的正向運動學計算思路來確定一個坐標系{2}相對于坐標系{0}的變換矩陣。最后得到在笛卡爾空間中的機械臂末端位置，即負載質(zhì)心的位置由下面的式子給出：

步驟5.動力學模型的建立。圖4所示的2自由度半柔性機械臂系統(tǒng)處于豎直平面內(nèi)。本發(fā)明采用遞推牛頓-歐拉動力學算法建立半柔性機械臂的動力學模型，即建立關節(jié)力矩、關節(jié)角、關節(jié)速度和關節(jié)加速度之間的關系。

用下面的式子簡寫正余弦函數(shù):

c₁＝cosθ₁,s₁＝sinθ₁,

記τ₁、τ₂分別為第1個關節(jié)和第2個關節(jié)的驅(qū)動力矩，g表示重力加速度。利用遞推牛頓-歐拉動力學算法得到整個機械臂系統(tǒng)的解析形式的動力學方程如下：

其中，

M₂₁＝M₁₂

C₂₂＝0

G₂＝m₂L₂gc₁₂

步驟6.分離干擾。為了更好的體現(xiàn)隨機干擾對機械臂系統(tǒng)的影響，下面把干擾分離出來。將式1帶入動力學方程得到

相應的各系數(shù)的擾動變化量可以分離出來。慣性矩陣各個分量的變化量為

ΔM₂₁＝ΔM₁₂

離心力和哥氏力矩陣各分量的變化量為

ΔC₂₂＝0

重力項的變化量為

ΔG₁＝m₂g[Δl₂cos(θ₁+θ₂+Δθ₂)-l₂sin(θ₁+θ₂)sinΔθ₂]

ΔG₂＝ΔG₁

記q＝[θ₁ θ₂]^T，分別表示關節(jié)角向量、關節(jié)角速度向量和關節(jié)角加速度向量，則分離出柔性干擾后的機械臂動力學方程如下：

其中，M⁰,C⁰,D⁰是機械臂變形前相應的動力學參數(shù)，擾動表示為

分離干擾后的運動學方程為：

X＝X⁰+ΔX

Y＝Y⁰+ΔY

其中，

X⁰＝l₁cosθ₁+l₂cos(θ₁+θ₂)

Y⁰＝l₁sinθ₁+l₂sin(θ₁+θ₂)

ΔX＝Δl₂cos(θ₁+θ₂)cosΔθ₂-(l₂+Δl₂)sin(θ₁+θ₂)sinΔθ₂

ΔY＝Δl₂sin(θ₁+θ₂)cosΔθ₂+(l₂+Δl₂)cos(θ₁+θ₂)sinΔθ₂

根據(jù)前面建立的運動學和動力學模型設計控制系統(tǒng)。

如圖4所示，本發(fā)明的運動控制系統(tǒng)的內(nèi)環(huán)采用魯棒自適應PD控制，實現(xiàn)關節(jié)角跟蹤，由線性PD反饋項、補償動力學模型的自適應控制項和補償干擾的魯棒控制項三部分組成。

本發(fā)明的運動控制系統(tǒng)的外環(huán)采用基于梯度下降法的開環(huán)迭代學習控制算法，具體的控制流程如圖5所示。

外環(huán)控制的目標是跟蹤笛卡爾空間的期望軌跡，定義笛卡爾空間的誤差向量為：

E＝[ΔX ΔY]^T＝[X-X^* Y-Y^*]^T

則一次運動過程的累積誤差平方和為

當機械臂的實際關節(jié)軌跡跟蹤上期望軌跡時，由于柔性干擾的存在，機械臂末端的軌跡變化并沒有跟蹤上期望的軌跡。為了在關節(jié)空間中尋找到合適的期望軌跡q_d以減少R的值到足夠小的，這里采用最優(yōu)化方法中的梯度下降的思想，把目標函數(shù)R對控制變量——期望軌跡q_d的負梯度方向作為調(diào)整期望關節(jié)角的方向，通過數(shù)次迭代學習可以使笛卡爾空間中的期望軌跡達到一定的精度要求。

記第k次和k+1次的期望關節(jié)軌跡分別為q^*(k)和q^*(k+1)，Δq^*(k+1)為第k+1次相對于第k次期望關節(jié)軌跡的調(diào)整量，則

q^*(k+1)＝q^*(k)+Δq^*(k+1)

現(xiàn)在需要確定Δq^*(k+1)，先求出目標函數(shù)R對控制變量的各個分量的負梯度方向分別為：

這樣就可以得到學習控制算法的表達式如下：

其中μ為學習步長。

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