專利名稱：履帶式移動(dòng)機(jī)械手的廣義系統(tǒng)統(tǒng)一動(dòng)力學(xué)建模方法
專利說明履帶式移動(dòng)機(jī)械手的廣義系統(tǒng)統(tǒng)一動(dòng)力學(xué)建模方法 技術(shù)領(lǐng)域：
本發(fā)明涉及移動(dòng)機(jī)械手的控制技術(shù)領(lǐng)域。
本方法主要涉及一種移動(dòng)機(jī)械手的統(tǒng)一動(dòng)力學(xué)建模方法，特別是用廣義系統(tǒng)理論對(duì)一類非完整移動(dòng)機(jī)器人進(jìn)行動(dòng)力學(xué)建模和控制，可用于實(shí)體移動(dòng)機(jī)械手的控制器設(shè)計(jì)。
背景技術(shù)：
履帶式移動(dòng)機(jī)械手系統(tǒng)是由一個(gè)機(jī)械手固定在一個(gè)履帶式移動(dòng)平臺(tái)上構(gòu)成。目前，機(jī)械手已被廣泛應(yīng)用于流水生產(chǎn)線產(chǎn)品的抓取操作上。移動(dòng)機(jī)械手同時(shí)具有移動(dòng)和操作兩大功能。平臺(tái)的移動(dòng)擴(kuò)大了機(jī)械手的工作空間，使機(jī)械手具有幾乎無限大的操作空間且能以更加適合的姿態(tài)來執(zhí)行任務(wù)。
一.建立動(dòng)力學(xué)模型是實(shí)時(shí)控制的需要 移動(dòng)機(jī)器人的數(shù)學(xué)模型包括運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型兩部分。建立動(dòng)力學(xué)模型是實(shí)時(shí)控制的需要，利用它可以實(shí)現(xiàn)最優(yōu)控制，達(dá)到良好的動(dòng)態(tài)性能和最優(yōu)指標(biāo)。目前對(duì)于移動(dòng)機(jī)械手的控制大多集中在運(yùn)動(dòng)學(xué)方面，控制輸入為系統(tǒng)的速度，此類控制方法存在如下問題(1)完全速度跟蹤假設(shè)在實(shí)際中并不成立；(2)忽略了移動(dòng)機(jī)械手所受干擾對(duì)控制系統(tǒng)的影響。這種運(yùn)動(dòng)學(xué)控制器在系統(tǒng)運(yùn)動(dòng)速度不大，無干擾或干擾很小的情況下還基本能夠保證系統(tǒng)的性能，但當(dāng)系統(tǒng)的運(yùn)動(dòng)速度增大，不確定性較強(qiáng)的情況下，很難保證控制性能。移動(dòng)機(jī)械手的不確定性主要來自兩個(gè)方面(1)移動(dòng)平臺(tái)與操作手的相互耦合作用而引起的模型的不精確；(2)由于動(dòng)態(tài)的、非結(jié)構(gòu)化的環(huán)境因素使移動(dòng)機(jī)械手受到的隨機(jī)干擾，如地面的不規(guī)則，輪子的滑動(dòng)等。
系統(tǒng)的實(shí)際控制輸入是由驅(qū)動(dòng)器產(chǎn)生的，從動(dòng)力層面考慮系統(tǒng)的控制問題與實(shí)際相符，設(shè)計(jì)動(dòng)力學(xué)控制器將會(huì)使系統(tǒng)性能和可實(shí)現(xiàn)性極大提高。
二.機(jī)械手和移動(dòng)平臺(tái)的強(qiáng)耦合作用要求設(shè)計(jì)移動(dòng)機(jī)械手的統(tǒng)一控制器 目前，國內(nèi)外對(duì)移動(dòng)機(jī)械手的控制主要采取分別控制的思想。其代表性研究有(1)YamamotoY用“首選操作區(qū)”的概念實(shí)現(xiàn)了機(jī)械手末端跟蹤一個(gè)運(yùn)動(dòng)物體的表面，其實(shí)質(zhì)是僅考慮機(jī)械手一個(gè)子系統(tǒng)的控制；(2)GoldenbergAA為移動(dòng)機(jī)械手的兩個(gè)子系統(tǒng)分別設(shè)計(jì)了基于神經(jīng)網(wǎng)絡(luò)的控制器，提出了需要較少控制參數(shù)的魯棒阻尼控制；(3)Chung J H利用非線性交互控制進(jìn)行運(yùn)動(dòng)學(xué)冗余度求解，分別設(shè)計(jì)了用于機(jī)械手控制的魯棒自適應(yīng)控制器和用于移動(dòng)載體的線性化控制器。上述分散控制的方法可以降低控制器設(shè)計(jì)的難度，但由于工業(yè)機(jī)械手質(zhì)量大，運(yùn)動(dòng)速度快，運(yùn)動(dòng)過程中會(huì)產(chǎn)生很大的耦合力作用在移動(dòng)平臺(tái)上；同時(shí)移動(dòng)平臺(tái)的運(yùn)動(dòng)也會(huì)干擾機(jī)械手的控制，影響控制精度。為此有學(xué)者嘗試將移動(dòng)機(jī)械手看成一個(gè)整體進(jìn)行控制。其代表性的研究有(1)Jagannathan S通過建立離散的模糊邏輯系統(tǒng)來逼近整個(gè)系統(tǒng)的動(dòng)力學(xué)模型，實(shí)現(xiàn)移動(dòng)載體和機(jī)械手的位置和速度跟蹤。其優(yōu)點(diǎn)是可以部分克服動(dòng)力學(xué)耦合和外部擾動(dòng)的影響，但其不足之處是必須滿足持續(xù)的激勵(lì)條件；(2)Yamamoto Y利用非線性反饋對(duì)耦合進(jìn)行集中補(bǔ)償，但實(shí)驗(yàn)證明這種補(bǔ)償對(duì)于由平臺(tái)移動(dòng)引起的跟蹤誤差效果較好，但很難補(bǔ)償由于機(jī)械手的運(yùn)動(dòng)引起的跟蹤誤差。
目前，對(duì)履帶式移動(dòng)機(jī)械手采用統(tǒng)一的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)建模加以控制的研究很少，已有方法對(duì)移動(dòng)平臺(tái)的非完整約束也缺乏有效的措施，為了解決上述問題，本發(fā)明提供了一種履帶式移動(dòng)機(jī)械手的廣義系統(tǒng)統(tǒng)一動(dòng)力學(xué)建模方法。

發(fā)明內(nèi)容
本發(fā)明目的是克服現(xiàn)有技術(shù)存在的上述不足，提供一種履帶式移動(dòng)機(jī)械手的廣義系統(tǒng)統(tǒng)一動(dòng)力學(xué)建模方法。
本發(fā)明所采用的技術(shù)方案是首先利用絕對(duì)坐標(biāo)計(jì)算履帶式移動(dòng)機(jī)械手的動(dòng)能和勢能；接著運(yùn)用機(jī)器人的拉格朗日動(dòng)力學(xué)方法對(duì)履帶式移動(dòng)機(jī)械手進(jìn)行統(tǒng)一的動(dòng)力學(xué)建模；最后基于移動(dòng)機(jī)械手的復(fù)雜性及移動(dòng)平臺(tái)具有非完整約束，將上述模型改寫為廣義系統(tǒng)下的統(tǒng)一動(dòng)力學(xué)模型，應(yīng)用廣義系統(tǒng)的控制理論對(duì)其進(jìn)行建模和研究。
具體建模方法如下 第一、歸納移動(dòng)機(jī)械手的結(jié)構(gòu)模型，即按照傳統(tǒng)的方法提取待研究的移動(dòng)機(jī)械手的主要特征， 得出其結(jié)構(gòu)圖，并將其放入絕對(duì)坐標(biāo)系中； 第二、確定各個(gè)關(guān)節(jié)的質(zhì)心絕對(duì)坐標(biāo)(xci，yci，zci)； 第三、計(jì)算整個(gè)系統(tǒng)的動(dòng)能 其中，用0表示履帶式移動(dòng)平臺(tái)的參數(shù)；i＝1-n分別表示機(jī)械手的n個(gè)關(guān)節(jié)， I為第i關(guān)節(jié)的轉(zhuǎn)動(dòng)慣量；Ωi為第i關(guān)節(jié)的角速度， vi為第i關(guān)節(jié)的質(zhì)心絕對(duì)線速度，即 對(duì)履帶式移動(dòng)平臺(tái)，設(shè)其質(zhì)心坐標(biāo)為xF，yF，則 kl，kr分別表示左右履帶相對(duì)于地面實(shí)際位移滑動(dòng)參數(shù)，這兩個(gè)參數(shù)的目的是為了引入對(duì)履帶-地面之間相互作用的一個(gè)估計(jì)； 第四、計(jì)算勢能 第五、計(jì)算拉格朗日函數(shù)L＝K-P； 第六、編程實(shí)現(xiàn) 第七、建立廣義系統(tǒng)模型 (1)、step6等式右邊整理可得 其中q＝(xF，yF，θP，θ1，θ2，θ3，θ4).， (2)、將2整理為 (3)、考慮非完整約束 建立廣義系統(tǒng)模型 其中 本發(fā)明的優(yōu)點(diǎn)和積極效果 本發(fā)明方法主要涉及一種移動(dòng)機(jī)械手的統(tǒng)一動(dòng)力學(xué)建模方法，特別是用廣義系統(tǒng)理論對(duì)一類非完整移動(dòng)機(jī)器人進(jìn)行動(dòng)力學(xué)建模和控制，其優(yōu)勢在于(1)可以有效克服原來方法中分別對(duì)移動(dòng)平臺(tái)和機(jī)械手進(jìn)行控制時(shí)，由于忽略移動(dòng)平臺(tái)和機(jī)械手之間的耦合力而造成的控制誤差；(2)說明書中給出的程序框圖可以應(yīng)用于任何類型的移動(dòng)機(jī)械手動(dòng)力學(xué)建模，使建模方法模式化，方便實(shí)施；(3)將非完整約束用廣義系統(tǒng)的代數(shù)方程來表示，可以直接用廣義系統(tǒng)已有的控制方法直接對(duì)這類移動(dòng)機(jī)械手設(shè)計(jì)控制器，有效的克服了非完整約束所帶來的控制難度。將此方法應(yīng)用于天津理工大學(xué)的一個(gè)履帶式移動(dòng)機(jī)械手上，控制效果比較理想(見實(shí)施例1)。


圖1是本發(fā)明所用機(jī)器人的結(jié)構(gòu)示意圖； 圖2是本發(fā)明的程序框圖； 圖3是采用本發(fā)明的方法建立模型后，用反饋線性化控制時(shí)得到的控制力矩，其中， 圖3-A，是移動(dòng)平臺(tái)上機(jī)械手的關(guān)節(jié)1的輸入力矩圖； 圖3-B，是移動(dòng)平臺(tái)上機(jī)械手的關(guān)節(jié)2的輸入力矩圖； 圖3-C，是移動(dòng)平臺(tái)上機(jī)械手的關(guān)節(jié)3的輸入力矩圖； 圖3-D，是移動(dòng)平臺(tái)上機(jī)械手的關(guān)節(jié)4的輸入力矩圖； 圖3-E，是移動(dòng)平臺(tái)右輪的輸入力矩圖； 圖3-F，是移動(dòng)平臺(tái)左輪的輸入力矩圖； 圖4是采用本發(fā)明的方祛建立模型后，用反饋線性化控制時(shí)得到的關(guān)節(jié)跟蹤曲線，其中， 圖4-A，是移動(dòng)平臺(tái)上機(jī)械手的關(guān)節(jié)1跟蹤階躍響應(yīng)的曲線； 圖4-B，是移動(dòng)平臺(tái)上機(jī)械手的關(guān)節(jié)2跟蹤階躍響應(yīng)的曲線； 圖4-C，是移動(dòng)平臺(tái)上機(jī)械手的關(guān)節(jié)3跟蹤階躍響應(yīng)的曲線； 圖4-D，是移動(dòng)平臺(tái)上機(jī)械手的關(guān)節(jié)4跟蹤階躍響應(yīng)的曲線； 圖4-E，是移動(dòng)平臺(tái)從起始點(diǎn)(-1，0)開始跟蹤(0，0)為圓心，半徑為2的圓的曲線。
具體實(shí)施方式
本發(fā)明提供的履帶式移動(dòng)機(jī)械手的廣義系統(tǒng)統(tǒng)一動(dòng)力學(xué)建模方法的具體步驟如下 第一、歸納移動(dòng)機(jī)械手的結(jié)構(gòu)模型，即按照傳統(tǒng)的方法提取待研究的移動(dòng)機(jī)械手的主要特征，得出其結(jié)構(gòu)圖，并將其放入絕對(duì)坐標(biāo)系中； 第二、確定各個(gè)關(guān)節(jié)的質(zhì)心絕對(duì)坐標(biāo)(xci，yci，zci)； 第三、計(jì)算整個(gè)系統(tǒng)的動(dòng)能 其中，用0表示履帶式移動(dòng)平臺(tái)的參數(shù)；i＝1-n分別表示機(jī)械手的n個(gè)關(guān)節(jié)， I為第i關(guān)節(jié)的轉(zhuǎn)動(dòng)慣量；Ωi為第i關(guān)節(jié)的角速度， vi為第i關(guān)節(jié)的質(zhì)心絕對(duì)線速度，即 對(duì)履帶式移動(dòng)平臺(tái)，設(shè)其質(zhì)心坐標(biāo)為xF，yF，則 kl，kr分別表示左右履帶相對(duì)于地面實(shí)際位移滑動(dòng)參數(shù)，這兩個(gè)參數(shù)的目的是為了引入對(duì)履帶一地面之間相互作用的一個(gè)估計(jì)； 第四、計(jì)算勢能 第五、計(jì)算拉格朗日函數(shù)L＝K-P； 第六、編程實(shí)現(xiàn) 第七、建立廣義系統(tǒng)模型 (1)、step6等式右邊整理可得 其中q＝(xF，yF，θP，θ1，θ2，θ3，θ4).， (2)、將2整理為 (3)、考慮非完整約束 建立廣義系統(tǒng)模型 其中 實(shí)施例1 具體參數(shù)如下 車體幾何參數(shù)r＝0.105；kl＝1；kr＝1；lf＝0.09；b＝0.205；l0＝0.6； 機(jī)械手幾何參數(shù)l1＝0.1；l2＝0.6；l3＝0.35；l4＝0.1； 機(jī)械手質(zhì)心位置pc1＝0.07/0.1；pc2＝0.27/0.6；pc3＝0.112/0.35；pc4＝0.031/0.1； 移動(dòng)機(jī)械手各關(guān)節(jié)質(zhì)心高度zc0＝0.25；ll0＝0.25；ll1＝0.6；zc1＝ll1+pc1*l1； 移動(dòng)機(jī)械手各關(guān)節(jié)質(zhì)量m0＝25；m1＝6.84；m2＝3.24；m3＝1.54；m4＝1.74； 重力加速度g＝9.8 移動(dòng)機(jī)械手各關(guān)節(jié)轉(zhuǎn)動(dòng)慣量j0＝0.5512；j1＝0.077；j2＝0.18；j3＝0.035；j4＝0.005 用說明書所述的方法建立廣義系統(tǒng)統(tǒng)一動(dòng)力學(xué)模型后，直接加入統(tǒng)一的PID協(xié)調(diào)控制器，取kp＝diag(50 50 50 30 30 30 30)，kd＝diag(100 100 100 100 100 100 100)，即可得到圖3，圖4的控制效果?？朔艘酝刂浦蟹謩e對(duì)移動(dòng)平臺(tái)和機(jī)械手建模，分別設(shè)計(jì)控制器時(shí)，由于忽略移動(dòng)平臺(tái)和機(jī)械手之間的耦合力而造成的控制誤差。
權(quán)利要求
1、一種履帶式移動(dòng)機(jī)械手的廣義系統(tǒng)統(tǒng)一動(dòng)力學(xué)建模方法，其特征在于該方法包括
第一、歸納移動(dòng)機(jī)械手的結(jié)構(gòu)模型，即按照傳統(tǒng)的方法提取待研究的移動(dòng)機(jī)械手的主要特征，得出其結(jié)構(gòu)圖，并將其放入絕對(duì)坐標(biāo)系中；
第二、確定各個(gè)關(guān)節(jié)的質(zhì)心絕對(duì)坐標(biāo)(xci，yci，zci)；
第三、計(jì)算整個(gè)系統(tǒng)的動(dòng)能
其中，用0表示履帶式移動(dòng)平臺(tái)的參數(shù)；i＝1-n分別表示機(jī)械手的n個(gè)關(guān)節(jié)，
I為第i關(guān)節(jié)的轉(zhuǎn)動(dòng)慣量；Ωi為第i關(guān)節(jié)的角速度，
vi為第i關(guān)節(jié)的質(zhì)心絕對(duì)線速度，即
對(duì)履帶式移動(dòng)平臺(tái)，設(shè)其質(zhì)心坐標(biāo)為xF，yF，則
kl，kr分別表示左右履帶相對(duì)于地面實(shí)際位移滑動(dòng)參數(shù)，這兩個(gè)參數(shù)的目的是為了引入對(duì)履帶-地面之間相互作用的一個(gè)估計(jì)；
第四、計(jì)算勢能
第五、計(jì)算拉格朗日函數(shù)L＝K-P；
第六、編程實(shí)現(xiàn)
第七、建立廣義系統(tǒng)模型
(1)、step6等式右邊整理可得
其中q＝(xF，yF，θP，θ1，θ2，θ3，θ4).，
(2)、將2整理為
(3)、考慮非完整約束
建立廣義系統(tǒng)模型
其中
全文摘要
本發(fā)明公開了一種履帶式移動(dòng)機(jī)械手的廣義系統(tǒng)統(tǒng)一動(dòng)力學(xué)建模方法。其核心包括利用絕對(duì)坐標(biāo)計(jì)算履帶式移動(dòng)機(jī)械手的動(dòng)能和勢能；運(yùn)用機(jī)器人的拉格朗日動(dòng)力學(xué)方法對(duì)履帶式移動(dòng)機(jī)械手進(jìn)行了統(tǒng)一的動(dòng)力學(xué)建模；由于移動(dòng)機(jī)械手是一個(gè)復(fù)雜的系統(tǒng)，且移動(dòng)平臺(tái)具有非完整約束，因此將上述模型改寫為廣義系統(tǒng)下的統(tǒng)一動(dòng)力學(xué)模型，應(yīng)用廣義系統(tǒng)的控制理論對(duì)其進(jìn)行研究。本發(fā)明避免了以往研究移動(dòng)式機(jī)器人時(shí)對(duì)移動(dòng)平臺(tái)和機(jī)械手分別建模，分別控制而忽略其耦合和相互影響所帶來的誤差，同時(shí)理論性強(qiáng)，思路清晰，編程容易實(shí)現(xiàn)，達(dá)到了對(duì)這類移動(dòng)機(jī)械手精確控制的目的。
文檔編號(hào)G05B17/00GK101526801SQ200910068500
公開日2009年9月9日 申請日期2009年4月17日 優(yōu)先權(quán)日2009年4月17日
發(fā)明者蔣文萍, 葛為民 申請人:天津理工大學(xué)