第第頁共43頁某智能液壓挖掘機精確軌跡跟蹤控制器設(shè)計目錄1緒論 11.1研究背景和意義 11.1.1國內(nèi)外挖掘機工作裝置控制方案研究現(xiàn)狀 11.2工作裝置控制方案研究現(xiàn)狀 21.3本文研究內(nèi)容 42挖掘機工作裝置運動學(xué)分析 52.1位姿描述與齊次變換 52.2挖掘機工作裝置D-H坐標(biāo)建立 62.3運動學(xué)分析 62.3.1運動學(xué)方程正向求解 62.3.2運動學(xué)方程逆向求解 72.4本章小結(jié) 83聯(lián)合仿真方案 93.1三維模型的建立 93.2三維模型導(dǎo)入MATLAB 103.3導(dǎo)入Simscape后的模型 113.4本章小結(jié) 124模型調(diào)試與手動控制器設(shè)計 134.1驅(qū)動的添加與信號轉(zhuǎn)換 134.2手動控制器仿真調(diào)試 144.3模型故障排除與封裝 154.4本章小結(jié) 175PID控制器的設(shè)計 185.1PID控制器概述 185.2Simulink中調(diào)試控制器 185.2.1P作用下的控制系統(tǒng)仿真 185.2.2PI作用下的控制系統(tǒng)仿真 195.2.3PID作用下的控制系統(tǒng)仿真 215.3仿真過程報錯解決辦法 225.4本章小結(jié) 236挖掘機軌跡跟蹤控制器 246.1元件的數(shù)學(xué)模型與控制系統(tǒng)框圖 256.1.1電液比例閥的傳遞函數(shù) 256.1.2液壓缸傳遞函數(shù) 256.2末端位姿實時顯示系統(tǒng) 256.3PID軌跡跟蹤控制 296.4本章小結(jié) 337機械臂實物模型建立 347.1舵機角度初始化 347.2模型搭建 357.3機械臂控制程序編寫 367.3.1串口通信 367.3.2控制代碼 377.4工程核算 387.5本章小結(jié) 388總結(jié)與展望 398.1本文總結(jié) 398.2展望 39參考文獻(xiàn) 411緒論1.1研究背景和意義液壓挖掘機作為一種重要的工程施工機械，廣泛應(yīng)用于建筑、道橋、軍事工程等領(lǐng)域。據(jù)統(tǒng)計世界上各種土方工程中約65％～70％是由液壓挖掘機來完成的[1]。為了提高挖掘機的工作效率并有效降低操作員的工作強度，國內(nèi)外各大廠商積極設(shè)法提升挖掘機的智能化水平，將自動控制技術(shù)應(yīng)用于挖掘機，工程設(shè)備的智能控制系統(tǒng)逐漸形成。挖掘機工作裝置的性能和運動狀態(tài)決定了液壓挖掘機的使用性能[2]。如何實現(xiàn)挖掘智能化是目前挖掘機自動控制研究的新方向，國內(nèi)外研究機構(gòu)及制造廠商致力于此方向的研究，主要包括自動無人挖掘、遠(yuǎn)程控制挖掘、故障診斷、挖掘機工作狀態(tài)實時監(jiān)控、控制系統(tǒng)自主學(xué)習(xí)等方面。其中挖掘機自動控制技術(shù)和軌跡規(guī)劃一直是困擾挖掘機智能化水平的技術(shù)難關(guān)。隨著機械電子學(xué)技術(shù)、自動化技術(shù)和無線通信技術(shù)的快速發(fā)展，逐漸實現(xiàn)了多功能智能挖掘機的構(gòu)思。通過控制工作裝置的末端來跟蹤規(guī)劃好的期望軌跡，期望軌跡被稱為軌跡控制中的目標(biāo)值，如:直線挖掘機，自動裝載等[3-4]。今年來，圍繞工作裝置軌跡跟蹤控制，學(xué)者們應(yīng)用不同的控制方法，提出了多種多樣的設(shè)計方案。主要的控制策略如下：①PID算法；②神經(jīng)網(wǎng)絡(luò)算法；③自適應(yīng)控制算法；④滑膜控制算法。本文以在建設(shè)工程中自動控制挖掘機的開發(fā)方向為焦點，以實現(xiàn)軌跡追蹤控制挖掘機作業(yè)裝置為目標(biāo)，使用SOLIDWORKS和MATLAB聯(lián)合協(xié)同仿真的策略，通過建立工作裝置運動學(xué)模型，把鏟斗末端的位置控制轉(zhuǎn)化為對關(guān)節(jié)角度的控制，設(shè)計完成工程機械精確軌跡跟蹤控制器，并對模型進(jìn)行控制仿真實驗，為進(jìn)一步實現(xiàn)挖掘機自動控制提供理論基礎(chǔ)。1.1.1國內(nèi)外挖掘機工作裝置控制方案研究現(xiàn)狀目前國內(nèi)外學(xué)者針對液壓伺服控制挖掘機存在非線性的特性[5]，對工作裝置的研究主要包括作業(yè)任務(wù)軌跡規(guī)劃以及工作裝置自動控制自動控制兩個方面。工作裝置作業(yè)任務(wù)軌跡規(guī)劃方面國外學(xué)者做了很多研究。F.A.Bender,S.Gltz等制定專用于小型液壓挖掘機多系統(tǒng)分析的物理模型以及專用于調(diào)整控制器參數(shù)的Hammerstein模型，針對自動控制系統(tǒng)運用了虛擬控制的方法進(jìn)行自動化控制研究[6]。A.Komissarov,Y.Lagunova等利用系統(tǒng)傳遞函數(shù)來代表主要機構(gòu)的性能特征從而對挖掘機在進(jìn)行額定作業(yè)過程中“從動機構(gòu)能量與力參數(shù)變化關(guān)系”進(jìn)行測評，最后通過仿真分析，推導(dǎo)總結(jié)出了挖掘機主要機構(gòu)間運動與力的傳遞函數(shù)解析表達(dá)式[7]。R.Mitrev,R.Gruychev等應(yīng)用AutodeskInventor平臺開發(fā)出可適用于挖掘機工作裝置動力學(xué)仿真功能模塊，該模塊能夠?qū)σ簤焊紫到y(tǒng)在工作循環(huán)過程中的軌跡和動力學(xué)與運動學(xué)參數(shù)進(jìn)行驗證分析[8]。S.P.Dimaio,S.E.Salcudean等開發(fā)了一種特別的重型液壓挖掘機模擬器，該模擬器支持對阻抗模型、氣缸工作模型和圖形用戶界面的仿真分析，能有效地評價挖掘機設(shè)計中的控制策略和操作環(huán)境[9]。南京工業(yè)大學(xué)胡海浪[10]等，利用機器人工具箱建立了挖掘機的運動仿真模型，為模擬挖掘機工作裝置的運動提供了一種有效的方法，為實現(xiàn)智能挖掘系統(tǒng)提供了運動學(xué)依據(jù)。1.2工作裝置控制方案研究現(xiàn)狀PID控制方案：PID控制因其設(shè)計簡單，在挖掘機控制領(lǐng)域得到了廣泛應(yīng)用[11]。東北大學(xué)劉闊等[12]采用傳統(tǒng)的PID控制實現(xiàn)了對挖掘機鏟斗軌跡的控制。西南科技大學(xué)袁亮[13]使用MATLAB、ADAMS和AMEsim軟件，三者進(jìn)行聯(lián)合仿真，以軌跡規(guī)劃中機械臂末端軌跡為控制目標(biāo)，采用PID控制算法實現(xiàn)機-液聯(lián)合仿真，仿真實驗結(jié)果說明聯(lián)合仿真模型的液壓控制系統(tǒng)使用PID控制器調(diào)節(jié)達(dá)到較好控制效果。而常規(guī)的PID控制則不適用于快速變化和波動的場合。當(dāng)被控對象運動速度過快或負(fù)載變化過大時，PID控制的精度無法保證，傳統(tǒng)的PID控制已不能滿足智挖掘機高精度作業(yè)的要求。優(yōu)化PID參數(shù)的控制方案：南京工業(yè)大學(xué)HaoFeng等[14]采用一種改進(jìn)的遺傳算法對挖掘機軌跡跟蹤PID控制參數(shù)進(jìn)行優(yōu)化，通過Ziegler-Nichols(Z-N)方法使初始種群在PID參數(shù)范圍內(nèi)產(chǎn)生，采用自主開發(fā)的適應(yīng)度函數(shù)、交叉和變異概率，避免了尋優(yōu)時過早收斂和停滯。吉林大學(xué)劉丹[15]通過對挖掘機工作裝置進(jìn)行的機器人正、逆運動學(xué)分析，得到鏟斗齒尖坐標(biāo)、各關(guān)節(jié)轉(zhuǎn)角、各油缸伸縮量的相互轉(zhuǎn)換關(guān)系，在此運動學(xué)模型的基礎(chǔ)上，采用五次多項式插值的方法對工作裝置的運動軌跡進(jìn)行優(yōu)化。AMESIM和MATLAB共同模擬平臺用于建立控制系統(tǒng)，采用遺傳算法優(yōu)化PID的控制方法和采用粒子群算法優(yōu)化的PID的控制方法進(jìn)行對比。后以正弦和階躍信號分別作為系統(tǒng)的輸入信號，得到正弦和階躍跟蹤曲線，通過對比實驗曲線，分析發(fā)現(xiàn)采用粒子群算法整定的PID控制器魯棒性更強，對系統(tǒng)的位置控制效果更為理想。在此基礎(chǔ)上為了進(jìn)一步驗證所提出的粒子群算法整定的PID控制策略的有效性，實驗分別以水平直線運動與斜坡運動為例，將兩種工況下各液壓缸的位移作為輸入信號進(jìn)行對比聯(lián)合仿真，得到三種控制方法下各液壓缸的位移軌跡跟蹤曲線與鏟斗齒尖軌跡跟蹤曲線。通過兩種實驗證明了采用粒子群算法優(yōu)化PID參數(shù)的控制方法具有很好的控制效果。江蘇城市職業(yè)學(xué)院夏正兵[16]，根據(jù)開環(huán)控制系統(tǒng)的傳遞函數(shù)，采用colony算法和模糊控制算法對PID控制器的參數(shù)進(jìn)行優(yōu)化，最終得到的正弦信號與模糊控制算法相比減少了超調(diào)，提高了系統(tǒng)的魯棒性?；赾olony算法的正弦信號系統(tǒng)的穩(wěn)態(tài)誤差、最大跟蹤誤差和平均跟蹤誤差均減少5%以上?；た刂扑惴ǎ夯？刂剖且环N能在一定程度上克服系統(tǒng)非線性和不確定性的魯棒控制方法。沈陽化工大學(xué)張金萍等[17]基于拉格朗日方法，確定了挖掘機四自由度（包括旋轉(zhuǎn)和回轉(zhuǎn)）的動力學(xué)方程，提出了一種自適應(yīng)模糊滑?？刂疲米赃m應(yīng)模糊控制的輸出時間改變滑?？刂茣r間，減少了系統(tǒng)的不穩(wěn)定。江南大學(xué)吳定會等[18]設(shè)計了一種適用于機械臂模型的改進(jìn)固定時間滑模軌跡跟蹤控制器。并通過對挖掘機工作裝置軌跡跟蹤控制的仿真，驗證了機械臂控制系統(tǒng)在受未知擾動影響時，改進(jìn)固定時間滑?？刂破髂鼙憩F(xiàn)出良好的軌跡跟蹤效果，在初始跟蹤誤差處于不同值時，跟蹤誤差均能在給定的固定時間內(nèi)達(dá)到收斂并穩(wěn)定。綜上所述，滑模控制的主要缺點是系統(tǒng)的不穩(wěn)定現(xiàn)象，如果為了克服系統(tǒng)的不確定性，負(fù)載突然變化，就必須提高系統(tǒng)的性能。但是這樣會引起系統(tǒng)飽和，甚至破壞系統(tǒng)的穩(wěn)定性。自適應(yīng)魯棒控制：魯棒控制適用于小范圍內(nèi)快速波動的不確定性，自適應(yīng)控制適用于大范圍內(nèi)小波動的不確定性。二者結(jié)合則優(yōu)勢互補。中南大學(xué)的張大慶等[19]采用實驗與理論相結(jié)合的方案，在一定程度上簡化了挖掘機電液比例控制的模型，保留了控制器參考模型，與經(jīng)典PID控制相比，自適應(yīng)控制器的最大傾斜度誤差為13.8%，最大傾斜跟蹤誤差為6.8%，自適應(yīng)控制在一定程度上提高了控制精度和穩(wěn)態(tài)響應(yīng)速度，表明整個系統(tǒng)具有良好的穩(wěn)定性和魯棒性。運用神經(jīng)網(wǎng)絡(luò)的控制方法：針對液壓挖掘機工作裝置末端軌跡跟蹤控制方案，中南大學(xué)賀繼林等[20]建立了智能挖掘機工作裝置運動學(xué)和動力學(xué)模型，并搭建了模型的MATLAB程序，克服了多變量、強耦合及負(fù)載擾動對于軌跡控制的影響，同時運用小腦模型神經(jīng)網(wǎng)絡(luò)(CMAC)的控制策略，設(shè)計了以時間為CMAC地址輸入矢量，進(jìn)而構(gòu)成CMAC神經(jīng)網(wǎng)絡(luò)控制器，逼近工作裝置動力學(xué)模型，很好的解決了自動軌跡控制的問題。最后，利用神經(jīng)網(wǎng)絡(luò)CMAC控制器對Adams中的山河智能挖掘機虛擬樣機的最終軌跡進(jìn)行控制。最大跟蹤誤差出現(xiàn)在前端的液壓缸上，最大跟蹤誤差為1.1mm，跟蹤誤差在允許范圍內(nèi)。1.3本文研究內(nèi)容本研究以挖掘機模型為研究對象，對其進(jìn)行運動學(xué)分析，實現(xiàn)對工作軌跡的控制，并利用SolidWorks和MATLAB協(xié)同仿真，實時顯示仿真的最終坐標(biāo)。最后使用Arduino單片機建立機械臂模型，編寫控制程序代碼，為實現(xiàn)實物控制打下基礎(chǔ)。本文研究內(nèi)容如下：（1）建立工程機械工作裝置運動學(xué)模型。（2）使用插件將三維模型導(dǎo)入MATLAB。（3）在Simscape中搭建針對工作裝置的滑塊拖動式控制系統(tǒng)，實現(xiàn)對仿真模型的調(diào)試。（4）搭建針對工作裝置末端的軌跡跟蹤控制器，實現(xiàn)末端坐標(biāo)實時輸出。（5）搭建機械臂模型，使用Arduino單片機編寫程序代碼，實現(xiàn)機械臂模型控制。

2挖掘機工作裝置運動學(xué)分析本章首先對挖掘機進(jìn)行了運動學(xué)分析，通過求解正向運動方程，確定了挖掘機末端位置與關(guān)節(jié)角的轉(zhuǎn)換關(guān)系。通過求解逆運動學(xué)方程，可以由工作裝置末端位置變化求解出各關(guān)節(jié)的轉(zhuǎn)角。2.1位姿描述與齊次變換剛體參考點的位置和剛體的姿態(tài)統(tǒng)稱為剛體的位姿，其描述方法較多，如齊次變換法、矢量法、旋量法和四次元數(shù)法等。本章采用齊次法實現(xiàn)挖掘機工作裝置的運動學(xué)模型建立。方位的描述采用旋轉(zhuǎn)矩陣進(jìn)行描述，繞x軸、y軸和z軸旋轉(zhuǎn)θ角的旋轉(zhuǎn)矩陣分別為：RRR式2.1坐標(biāo)軸旋轉(zhuǎn)矩陣2.2挖掘機工作裝置D-H坐標(biāo)建立圖2.1挖掘機工作裝置的連桿坐標(biāo)系簡圖挖掘機的工作部分可視為一個開放的運動鏈。為了在挖掘機進(jìn)行路線控制之前，描述各個工作區(qū)域的公共變量與鏟頂位置和位置之間的關(guān)系，有必要確定挖掘機的運動方程。為了建立每條連桿的公共變量與挖掘機鏟斗路徑之間的聯(lián)系，挖掘設(shè)備的關(guān)節(jié)坐標(biāo)參數(shù)圖如圖2.1所示。表2.1工作裝置D-H坐標(biāo)參數(shù)第i個關(guān)節(jié)關(guān)節(jié)活動范圍（°）液壓缸伸縮量（mm）ai（mm）110°～80°32mm～122mm510mm2-120～-55°52mm～105mm269mm380°～120°28mm～103mm165mm2.3運動學(xué)分析基于上節(jié)的D-H坐標(biāo)參數(shù)，利用D-H坐標(biāo)變形矩陣對挖掘機的前端進(jìn)行了運動學(xué)分析和求解，現(xiàn)用幾何方法對工作裝置的逆運動學(xué)進(jìn)行分析。2.3.1運動學(xué)方程正向求解對掘進(jìn)機作業(yè)裝置的每個鏈接設(shè)定坐標(biāo)系，用齊次變換記述了這些坐標(biāo)系之間的相對位置和姿勢。通過遞歸獲得基坐標(biāo)系端的均勻變換矩陣，即求得了機械臂的運動方程。圖2.2坐標(biāo)變換關(guān)系簡圖i式2.2坐標(biāo)變換矩陣由于挖掘機工作和回轉(zhuǎn)并不會同時進(jìn)行，所以在建立正解方程時不考慮回轉(zhuǎn)。參考圖2.2的坐標(biāo)變換關(guān)系簡圖，可以得到工作臂的運動學(xué)方程，如式2.3。x=y=式2.3工作裝置的運動學(xué)方程2.3.2運動學(xué)方程逆向求解采用幾何法求解工作裝置逆運動學(xué)解，需將工作裝置的空間幾何參數(shù)分解成碰面幾何參數(shù)，由此簡化的工作裝置簡圖如圖2.3所示。對于下圖所示的工作裝置模型簡圖，由余弦定理可以得到x2a=arccosL圖2.3幾何法求解簡圖求得連桿L1和L2之間的夾角α后，即可通過平面幾何關(guān)系求出θ1和θ2。θ2θ1至此我們即用幾何解法得到了挖掘機工作裝置的逆運動學(xué)全部解。2.4本章小結(jié)本章結(jié)合三維模型的參數(shù)，對工作裝置的運動進(jìn)行了分析，確定了工作裝置的正、逆運動學(xué)解，為后續(xù)的仿真提供了理論依據(jù)。

3聯(lián)合仿真方案在挖掘機工作裝置虛擬仿真方面，現(xiàn)有文獻(xiàn)實現(xiàn)仿真測試的途徑主要有一下幾種。（1）AMESim與MATLAB協(xié)同仿真在協(xié)同仿真過程中，將內(nèi)嵌在AMESim中的液壓系統(tǒng)模型轉(zhuǎn)化為內(nèi)嵌在Simulink模型中的函數(shù)，并將數(shù)據(jù)實時發(fā)送給控制器，控制器向AMESim發(fā)送信號。最后，仿真結(jié)果驗證了該方法的有效性，在協(xié)同仿真過程中，結(jié)合AMESim的優(yōu)點，仿真結(jié)果更接近實際情況。（2）ADAMS-AMEsim聯(lián)合仿真基于Adams和AMEsim的聯(lián)合仿真，在Adams中建立了液壓系統(tǒng)的數(shù)學(xué)模型和動力學(xué)模型，并通過集成接口軟件平臺進(jìn)行實時數(shù)據(jù)傳輸，通過在油壓循環(huán)系統(tǒng)中加入PID控制器，建立了完整的仿真模型機油壓力系統(tǒng)。（3）SOLIDWORKS與MATLAB聯(lián)合仿真本項目結(jié)合文獻(xiàn)中聯(lián)合仿真的基本方法，采用SOLIDWORKS與MATLAB聯(lián)合仿真的方法實現(xiàn)挖掘機工作裝置的控制仿真。簡化了工作裝置模型的仿真難度，避免了在聯(lián)合仿真過程中因模型復(fù)雜度而導(dǎo)致的仿真報錯問題。3.1三維模型的建立三維模型的建立是實現(xiàn)聯(lián)合仿真計劃的基礎(chǔ)。與SimScape中的參數(shù)化建模方法相比，該方法生成了美觀的可視化模型。使用SW建立貼近實際的模型對象再導(dǎo)入SimScape中，可以提高建模速度便于修改。繪制好各個零件后，并按實際完成模型的裝配，最后利用SOLIDWORKS模型美化插件對裝配體進(jìn)行渲染，得到的三維模型渲染圖如圖3.1所示。圖3.1挖掘機工作裝置渲染圖3.2三維模型導(dǎo)入MATLAB完成三維模型的建立后，要將模型導(dǎo)入MATLAB中實現(xiàn)聯(lián)合仿真。在這一過程中，首先要進(jìn)入MATLAB官網(wǎng)下載SimscapeMulitibody插件實現(xiàn)模型導(dǎo)入。下載好對應(yīng)MATLAB版本的插件安裝包之后，以管理員身份啟動MATLAB，并將下載好的插件文件夾路徑添加到MATLAB路徑中。之后在命令行窗口輸入install_addon（“smlink.r2019a.win64.zip”），左下角狀態(tài)提示欄運行狀態(tài)結(jié)束后依次運行命令regmatlabserver和smlink_linksw，全部運行成功后，插件安裝成功。此時打開SOLIDWORKS軟件，點擊菜單欄中的工具，在點擊插件，可以看到SimscapeMulitibodyLink插件已經(jīng)被成功安裝，如圖3.2所示。圖3.2導(dǎo)入后的插件界面3.3導(dǎo)入Simscape后的模型在SolidWorks中新建零件模型的保存格式是‘.sldprt’，裝配體完成后【工具】→【SimMechanics

Link】或者是【Simscape

Multibidy

Link】→【Export】→【SimMechanics

Link

Second

Generation】另存為.xlm文件，以備在MATLAB中打開。模型導(dǎo)入成功后會彈出如圖3.3所示的窗口。圖3.3導(dǎo)入成功窗口顯示啟動MATLAB，在命令行窗口輸入smimport('digger_sim.xml')，MATLAB將啟動Simulink并依據(jù)所繪制的三維模型參數(shù)，在Simscape中建立可視化系統(tǒng)模型對象，如圖3.4所示。圖3.4可視化系統(tǒng)模型對象3.4本章小結(jié)本章主要完成了聯(lián)合仿真方案的基礎(chǔ)工作，包括SOLIDWORKS三維模型的建立以及MATLAB中可視化系統(tǒng)模型對象的建立。在這一過程中使用到了SimscapeMulitibody插件實現(xiàn)模型導(dǎo)入，為以后在MATLAB中實現(xiàn)控制仿真打下了基礎(chǔ)。

4模型調(diào)試與手動控制器設(shè)計利用上一個章建立的可視化系統(tǒng)模型對象在Simulink中可以獲得如圖4.1所示的控制對象模型。此時以默認(rèn)狀態(tài)運行仿真模型，即仿真模式選擇Normal，仿真時長為10s，采用ode45算法。此時執(zhí)行仿真，由于模型未添加驅(qū)動，故仿真結(jié)果并非理想狀態(tài)。因此需要為模型添加驅(qū)動實現(xiàn)工作裝置手動模式下的可控運動。圖4.1控制對象模型4.1驅(qū)動的添加與信號轉(zhuǎn)換首先設(shè)置三個轉(zhuǎn)動副的輸入狀態(tài)，雙擊打開RevoluteJoint模塊完成關(guān)節(jié)的輸入扭矩和驅(qū)動運動設(shè)置。這里扭矩（Torque）采用電腦自動計算模式，驅(qū)動運動設(shè)置（motion）使用“外部提供模式”，如圖4.2所示。為方便在仿真過程中觀察工作裝置運動狀態(tài)，激活傳感器選項（sensing）中的Position，創(chuàng)建角度輸出接口。三個轉(zhuǎn)動副都做上述設(shè)置。圖4.2驅(qū)動模式設(shè)置之后調(diào)用Simulink模塊庫中的Simulink-PSConverter模塊實現(xiàn)無量綱函數(shù)到物理信號的轉(zhuǎn)換，將信號轉(zhuǎn)換為角度信號（deg）。為實現(xiàn)可視化對象在手動控制模式下能夠?qū)崿F(xiàn)即時運動控制，因此不能采用給予單一運動信號的常規(guī)調(diào)試模式。查閱相關(guān)模塊使用方法后，確定了一種組合模塊實現(xiàn)驅(qū)動的方案設(shè)計。即調(diào)用Constant模塊組合SliderGain模塊實現(xiàn)仿真狀態(tài)下的工作裝置的滑動控制，要注意將SliderGain模塊的上下限調(diào)整為第二章規(guī)定的運動范圍，保證仿真情況與實際物理模型一致。利用Mux模塊將各個關(guān)節(jié)的角位移輸出到示波器中顯示。搭建好的工作裝置手動控制系統(tǒng)如圖4.3所示。圖4.3工作裝置手動調(diào)試系統(tǒng)4.2手動控制器仿真調(diào)試手動控制器是實現(xiàn)前期被控對象模型調(diào)試的基礎(chǔ)。在執(zhí)行仿真任務(wù)前，需要對仿真方式進(jìn)行設(shè)置。首先將仿真時長設(shè)置為無限時長（inf），之后在設(shè)置中將求解器設(shè)置為固定步長，設(shè)置大小為0.001。點擊運行按鈕運行手動控制系統(tǒng)模型，仿真狀態(tài)如圖4.4。圖4.4仿真狀態(tài)圖啟動仿真后，發(fā)現(xiàn)滑動控制器被拖動后沒有有出現(xiàn)預(yù)想情況下的即拖即動。在排除問題的過程中發(fā)現(xiàn)是在Simscape多體動力學(xué)仿真界面中仿真時間倍數(shù)未做修改，導(dǎo)致手動控制器在仿真界面顯示的狀態(tài)有較大滯后。調(diào)整仿真時間倍數(shù)為X256倍后，仿真效果達(dá)到預(yù)想效果。4.3模型故障排除與封裝在手動控制系統(tǒng)的建立和仿真過程中，遇到了因信號轉(zhuǎn)換模塊報錯導(dǎo)致的模型編譯失敗的問題。報錯系統(tǒng)提示如圖4.5所示。圖4.5故障報錯界面查閱有關(guān)資料后，確定問題是由于Slover求解器設(shè)置不對導(dǎo)致的。打開PSConvertor模塊，1.把Simulink-PS

converter模塊里面的Input

Handling參數(shù)分別改成Filterinput，derivatives

calculated，Inputfilteringorder選項改為Second-order

filtering，時間常數(shù)設(shè)置為0.001，設(shè)置好的界面如圖4.6所示。并對其余模型中用到的常規(guī)信號轉(zhuǎn)物理信號模塊都做上述處理。圖4.6信號轉(zhuǎn)換模塊參數(shù)設(shè)置修改完后再次運行控制系統(tǒng)，仿真模型工作正常。為實現(xiàn)后續(xù)自動控制系統(tǒng)的設(shè)計和仿真需要將已經(jīng)調(diào)試好的控制對象模型封裝為一個子系統(tǒng)，并創(chuàng)建所需要的的輸入輸出接口。創(chuàng)建好的子系統(tǒng)如圖4.7所示。圖4.7子系統(tǒng)的創(chuàng)建與封裝在子系統(tǒng)的創(chuàng)建和封裝過程中，除了對子系統(tǒng)添加適當(dāng)?shù)膬?nèi)容描述外，還可以為各個子系統(tǒng)添加圖片使模型表達(dá)的信息更加直觀。具體操作步驟為在子系統(tǒng)上單擊右鍵，選擇“Mask”選項中的“Add

mask

icon

image”，通過瀏覽選擇之前SOLIDWORKS制作的三維渲染圖。需要注意的是Icon

transparency選項要選擇Opaque

ports，否則，無法顯示子模塊的輸入和輸出針腳。4.4本章小結(jié)本章主要完成了針對挖掘機工作裝置的手動控制器設(shè)計，對在仿真調(diào)試過程中遇到的問題進(jìn)行解讀，提出解決方案，并對調(diào)試好的模型進(jìn)行了封裝和優(yōu)化。5PID控制器的設(shè)計5.1PID控制器概述PID控制器（ProportionIntegrationDifferentiation），由比例環(huán)節(jié)、積分環(huán)節(jié)和微分環(huán)節(jié)組成，在實際控制過程中，控制器不斷改變kp、Ki、Kd，PID控制系統(tǒng)可以將其看做閉環(huán)負(fù)反饋控制，其控制原理框圖如圖5.1所示。圖5.1PID控制系統(tǒng)框圖在PID控制中，kp、ki和kd的相互作用是互補的。如果比例常數(shù)、積分常數(shù)和微分常數(shù)設(shè)置正確，可以達(dá)到較好的控制效果。5.2Simulink中調(diào)試控制器運行Simulink并搭建基于控制對象的PID控制系統(tǒng)，控制系統(tǒng)框圖如圖5.2所示。設(shè)置輸入信號為10的階躍，示波器采集輸入PID控制器的前后信號。圖5.2控制系統(tǒng)方框圖5.2.1P作用下的控制系統(tǒng)仿真圖5.3P作用下的控制系統(tǒng)打開封裝好的PID控制器模型，將P設(shè)置為10，其余參數(shù)設(shè)置為0，運行系統(tǒng)并進(jìn)行仿真實驗，得到的結(jié)果如圖5.3所示。由仿真數(shù)據(jù)可以看出僅有P作用下的控制系統(tǒng)沒能消除穩(wěn)態(tài)誤差，同時輸出曲線與誤差的曲線相比較存在著固定的比例關(guān)系。同時調(diào)整P參數(shù)，將其擴(kuò)大為100，如圖5.4所示，可以看到控制器輸出上下限更大，達(dá)到穩(wěn)定狀態(tài)是的余差更小，達(dá)到穩(wěn)定狀態(tài)的振蕩周期也變短。然而，隨著P的增加，系統(tǒng)的穩(wěn)定性也降低，可以描述為從衰減振動到臨界振動的轉(zhuǎn)變過程，直到發(fā)散振蕩。圖5.4放大P參數(shù)后的仿真結(jié)果5.2.2PI作用下的控制系統(tǒng)仿真復(fù)制控制系統(tǒng)，修改新創(chuàng)建的控制系統(tǒng)PID控制器的P為10，I為5。接入Mux模塊實現(xiàn)兩個系統(tǒng)的仿真效果對比，搭建的系統(tǒng)如圖5.5所示。運行Simulink，得到的仿真結(jié)果如圖5.6所示。仿真結(jié)果表明，與p控制相比，pi控制去除了穩(wěn)態(tài)誤差，但系統(tǒng)的超調(diào)和穩(wěn)定時間大于p控制下的控制系統(tǒng)。圖5.5更新后的控制系統(tǒng)圖5.6對比仿真結(jié)果在控制系統(tǒng)中，若保持控制器的比例系數(shù)不變，改變積分常數(shù)為7。從圖5.7中可以看出，積分時間對過渡過程的影響是具有兩重性的:積分效果增大時，在相同的干擾下控制器的輸出增加，最大偏差減少，余留消除加快。然而，同時，系統(tǒng)的振動變得激烈，穩(wěn)定性降低，系統(tǒng)的不穩(wěn)定性也發(fā)生。在比例控制系統(tǒng)中，通過添加積分函數(shù)會降低系統(tǒng)的穩(wěn)定性。因此，為了維持本來的穩(wěn)定性，需要根據(jù)積分常數(shù)適當(dāng)?shù)卣{(diào)整比例常數(shù)。這樣，系統(tǒng)的振蕩周期、控制時間和最大偏差變大。積分作用的引人，一方面起到了消除余差的作用，而另一方面卻對控制系統(tǒng)的其他品質(zhì)指標(biāo)造成負(fù)面影響。圖5.7修改參數(shù)后的仿真結(jié)果5.2.3PID作用下的控制系統(tǒng)仿真同理可搭建PID參數(shù)分別為10,5,3的控制系統(tǒng)，Simulink中搭建的模型如圖5.8所示。運行得到的仿真結(jié)果如圖5.9。根據(jù)仿真結(jié)果可以看出相比于前兩種仿真結(jié)果，該控制器的超調(diào)量較小，穩(wěn)定時間也較短。圖5.8組合控制系統(tǒng)圖5.9PID控制系統(tǒng)仿真結(jié)果對于大慣性的被控對象，如果能根據(jù)被控對象的變化率提前確定控制策略，而不是等待反饋。可以說該控制器具有良好的可預(yù)測性，可做到提前控制。因此在控制系統(tǒng)中適當(dāng)調(diào)整微分參數(shù)可以全面改善控制系統(tǒng)的品質(zhì)指標(biāo)。三種控制系統(tǒng)使用Mux總線輸出到一個示波器中實現(xiàn)控制結(jié)果仿真對比，如圖5.10所示。由仿真結(jié)果不難看出，由于微分項對于突變特別敏感，因此在真實狀況下，系統(tǒng)可能無法輸出仿真情況下的值。圖5.10三種控制系統(tǒng)仿真輸出5.3仿真過程報錯解決辦法搭建控制系統(tǒng)，并對其進(jìn)行單獨仿真的過程中，程序采用默認(rèn)求解模式都可以正常運行。將三種控制系統(tǒng)放入一個Simulink模型中并計劃用Mux總線實現(xiàn)三種控制方案對比時，出現(xiàn)了如圖5.11所示的報錯界面。圖5.11報錯界面后參考MATLAB官方論壇，確定問題是由于求解器設(shè)置有誤導(dǎo)致的。具體解決方案如下，首先“ctrl+E”進(jìn)入求解器設(shè)置界面，修改采用剛性算法ode23tb進(jìn)行求解。5.4本章小結(jié)本章主要對Simulink中搭建的PID控制模型進(jìn)行了不同參數(shù)狀態(tài)下的仿真調(diào)試。對PID控制器三大參數(shù)對控制效果的影響有了實際認(rèn)識。為后續(xù)針對挖掘機工作裝置PID控制建立了理論基礎(chǔ)。

6挖掘機軌跡跟蹤控制器本章將完成挖掘機軌跡跟蹤控制系統(tǒng)的搭建，具體方案流程圖如圖6.1所示。其中運動學(xué)模型的建立工作已在第二章部分完成。本章需要對Simulink中調(diào)試好的模型添加PID控制器、建立各元件的傳遞函數(shù)并實現(xiàn)挖掘機工作裝置末端位置的實時顯示。圖6.1控制系統(tǒng)流程圖6.1元件的數(shù)學(xué)模型與控制系統(tǒng)框圖6.1.1電液比例閥的傳遞函數(shù)電液比例閥是實現(xiàn)挖掘機液壓控制系統(tǒng)的核心原件之一，可將其視為一個一階慣性環(huán)節(jié)，因此其傳遞函數(shù)可以近似表示為：G對于雙向比例調(diào)速閥2FRE6，根據(jù)手冊顯示電液比例閥的特性是比例閥的額定流量為1Hz。最大線圈輸入電流為2200mA。把各項參數(shù)帶入式6.1得其傳遞函數(shù)為G6.1.2液壓缸傳遞函數(shù)對于挖掘機工作裝置液壓缸，其傳遞函數(shù)為：G式6.3中，ξn為阻尼比，ξn=4βe×A2Vt×m；A-活塞平均面積，單位m2；Kq對于液壓缸，其傳遞函數(shù)為：G其余環(huán)節(jié)可視為比例環(huán)節(jié)。工作裝置的控制系統(tǒng)框圖如圖6.2所示。圖6.2工作裝置控制系統(tǒng)框圖6.2末端位姿實時顯示系統(tǒng)用第三章第四章的多體動力學(xué)聯(lián)合仿真模型，在Simulink模型中調(diào)用模塊庫中的TransformSenor模塊可以實現(xiàn)工作裝置末端位姿實時顯示。圖6.3調(diào)用位姿傳感器模塊為顯示工作裝置末端實時位姿，需要打開位姿傳感器模塊進(jìn)行相應(yīng)設(shè)置。勾選Rotation選項卡下的Quaternion，端口Q可輸出從動架相對于基架的純旋轉(zhuǎn)的單元四元數(shù)，即可顯示相對于基準(zhǔn)坐標(biāo)系的旋轉(zhuǎn)角。勾選Translation選項卡下的X、Y、Z，可創(chuàng)建X、Y、Z，三個輸出端口實現(xiàn)末端位置的實時顯示，模塊設(shè)置界面如圖6.4所示。圖6.4模塊設(shè)置界面此時搭建的末端顯示系統(tǒng)還無法將信息完全顯示，需要組合其他模塊完成末端位姿顯示系統(tǒng)的搭建，搭建好的末端位姿顯示系統(tǒng)如圖6.5所示。首先使用信號轉(zhuǎn)換模塊，將Simulink輸出的信號賦予實際物理意義，其中位姿顯示部分設(shè)置單位為mm，姿態(tài)部分設(shè)置為角度deg。兩部分都需要接入Display模塊實現(xiàn)數(shù)據(jù)的實時輸出，其中位置坐標(biāo)要用Mux總線進(jìn)行連接實現(xiàn)三維坐標(biāo)整體輸出。圖6.5末端位姿顯示系統(tǒng)由于在先前的工作中已經(jīng)將裝置模型進(jìn)行封裝，為方便在后續(xù)系統(tǒng)調(diào)試過程中在主界面觀察末端位置輸出，這里插入out模塊建立一個封裝輸出接口，搭建好的模型如圖6.6所示。圖6.6具備末端位置顯示功能的控制模型設(shè)置仿真模式為默認(rèn)模式，仿真時長為10s。輸入信號采用如圖6.7所示的正弦信號激勵下的角度輸入信號。在主界面子系統(tǒng)與仿真模型的接法如圖6.8所示。在啟動仿真前，需要將開關(guān)按圖示界面使封裝好的輸入信號子系統(tǒng)與仿真模型子系統(tǒng)輸入接口相連接。圖6.7輸入信號子系統(tǒng)圖6.8子系統(tǒng)與仿真模型的接法啟動仿真后在主界面可以觀察到輸出的工作裝置末端實時坐標(biāo)。打開封裝好的仿真模型，可看到如圖6.9所示的位姿顯示界面。同時可以觀察到在MATLAB主界面工作區(qū)輸出了仿真過程三維動畫，如圖6.10所示。圖6.9位姿顯示界面圖6.10三維動畫輸出6.3PID軌跡跟蹤控制本小節(jié)主要實現(xiàn)挖掘機工作裝置的軌跡跟蹤控制。在給模型加入PID控制器前，需要將先前設(shè)置的三個轉(zhuǎn)動副驅(qū)動方式進(jìn)行調(diào)整。Actuation驅(qū)動選項卡中的扭矩改為這里扭矩（Torque）采用“外部提供模式”，驅(qū)動運動設(shè)置（motion）使用“電腦自動計算模式”，如圖6.11所示。圖6.11轉(zhuǎn)動副驅(qū)動設(shè)置在主界面搭建如圖6.12所示的PID控制回路。并對每一個轉(zhuǎn)動副進(jìn)行單獨的PID參數(shù)調(diào)試，最后進(jìn)行疊加。圖6.12仿真控制系統(tǒng)在進(jìn)行PID參數(shù)調(diào)試的過程中，采用了PID模塊集成的自動調(diào)參功能。雙擊打開PID模塊，點擊Tune選項，系統(tǒng)會編譯模型，并自動調(diào)整參數(shù)，如圖6.13和6.14所示。之后滑動頂端的參數(shù)調(diào)整器對階躍響應(yīng)進(jìn)行微調(diào)。圖6.13PID參數(shù)自動整定圖6.14調(diào)整前的階躍響應(yīng)經(jīng)過參數(shù)調(diào)整，前后的階躍響應(yīng)曲線對比如圖6.15所示。調(diào)整后的PID參數(shù)分別為1.6915、0.232、-0.21936。可以看到經(jīng)過調(diào)整后的控制系統(tǒng)響應(yīng)時間可以調(diào)整到0.825s，超調(diào)量為10.6%，如圖6.16所示。圖6.15調(diào)整參數(shù)后的階躍響應(yīng)圖6.16控制系統(tǒng)參數(shù)參數(shù)調(diào)整好后，設(shè)置仿真時間為10s，可以觀察到如圖6.17所示的輸出結(jié)果。圖6.17仿真結(jié)果6.4本章小結(jié)本章完成了工作裝置的控制系統(tǒng)，采用PID模塊參數(shù)自動整定的方法對PID參數(shù)進(jìn)行優(yōu)化，并在仿真模型中采用傳感器位置轉(zhuǎn)換模塊，實現(xiàn)工作裝置末端位置的實時顯示。7機械臂實物模型建立7.1舵機角度初始化在搭建實物模型并實現(xiàn)模型機械臂控制前，首先需要為四個機械臂關(guān)節(jié)的舵機規(guī)劃初始角度。給四個舵機的位號分別命名為base、rArm、fArm和claw。硬件接線方法如圖7.1所示，編寫的舵機初始化程序如圖7.2所示。圖7.1舵機初始化接線圖7.2舵機初始化程序7.2模型搭建完成舵機初始化操作后，根據(jù)圖紙搭建實物模型，搭建好的實物模型如圖7.3所示。模型采用亞克力板切割件，利用板件預(yù)留的孔位，用螺栓進(jìn)行緊固的模式組裝。圖7.3機械臂模型搭建模型的過程中用到的螺紋緊固件類型及數(shù)目如表7.1所示。搭建好的模型如圖7.4所示。表7.1緊固件類型與數(shù)目型號規(guī)格數(shù)目M3螺栓×66M3螺栓×815M3螺栓×103M3螺栓×128M3螺栓×254M3螺母10圖7.4實物模型7.3機械臂控制程序編寫本小節(jié)主要任務(wù)是編寫機械臂控制程序，通過Arduino與電腦通訊，實現(xiàn)機械臂裝置的運動控制。7.3.1串口通信筆記本電腦可以通過USB端口與Arduino的RX引腳和TX引腳實現(xiàn)通信。因此，調(diào)用Serial庫函數(shù)節(jié)課實現(xiàn)相關(guān)的串口通訊功能。以本程序為例，首先在Setup函數(shù)中調(diào)用Serial.begin（9600）設(shè)置串口波特率為9600，在之后的調(diào)試過程中將串口監(jiān)視器的波特率設(shè)置為9600即可實現(xiàn)命令的傳輸與信息反饋。Serial.println("Welcometouserobotarm.")實現(xiàn)正常通訊時串口監(jiān)視器顯示W(wǎng)elcometouserobotarm.字符串的功能。圖7.5串口初始化程序代碼7.3.2控制代碼在loop函數(shù)中，使用if判斷語句，其中判斷條件為串口是否有數(shù)據(jù)等待傳輸。若串口接收緩存非空，則調(diào)用Serial.read（）函數(shù)，將讀取的數(shù)據(jù)存放在serialCmd字符中，之后系統(tǒng)調(diào)用armDataCmd函數(shù)。圖7.6控制程序調(diào)用代碼以輸入串口命令b90為例，介紹armDataCmd函數(shù)執(zhí)行過程，函數(shù)部分代碼如圖7.7所示。輸入的指令b90首先字符b會被讀取并存儲到serialCmd字符變量中。舵機輸出角度90則被Serial.parseInt()函數(shù)讀取并存儲到servoData整型變量中。之后通過switch(serialCmd)語句，根據(jù)serialCmd中存儲的字符調(diào)用case'b'語句，并執(zhí)行basePos=servoData賦值語句更新base舵機的角度信息。跳出armDataCmd函數(shù)后，由于程序開始前已經(jīng)創(chuàng)建好base,fArm,rArm,claw四個舵機對象，執(zhí)行base.write(basePos)命令即可將串口監(jiān)視器中輸入的base舵機旋轉(zhuǎn)至90度位置的命令執(zhí)行。圖7.7armDataCmd函數(shù)部分代碼7.4工程核算挖掘機工作裝置機械臂模型主要分為控制系統(tǒng)和機械臂模型兩部分，搭建整個實物模型的開銷如表7.1所示。序號名稱單價/元數(shù)量/個總價/元1Arduino單片機12711272亞克力板（含加工費）12911293舵機74284M3螺栓組合2125面包板121126杜邦線212總計：300元7.5本章小結(jié)本章主要完成了機械臂實物模型的搭建，使用Arduino單片機編寫了針對機械臂的控制程序，實現(xiàn)了通過串口監(jiān)視器控制機械臂運動，并對搭建的模型進(jìn)行了工程核算。

8總結(jié)與展望8.1本文總結(jié)目前，本文通過SOLIDWORKS與MATLAB聯(lián)合仿真實現(xiàn)了挖掘機工作裝置的運動仿真與末端軌跡控制與顯示，并利用Arduino單片機編寫了機械臂模型控制程序?qū)崿F(xiàn)了通過串口監(jiān)視器控制機械臂運動。本文所做的具體工作如下：（1）在SOLIDWORKS中搭建挖掘機工作裝置模型，求解工作裝置正逆運動學(xué)解，并對模型進(jìn)行渲染和導(dǎo)出。（2）將三維模型導(dǎo)入SimScape中，使用Simulink中的各種模塊組合，建立工作裝置手動控制器。（3）用Simulink對PID控制器進(jìn)行了仿真和調(diào)試，為以后控制器接入物理系統(tǒng)打下基礎(chǔ)。（4）用Simulink中機器人工具箱部分模塊，搭建針對工作裝置末端的軌跡跟蹤控制器，實現(xiàn)末端坐標(biāo)實時輸出。（5）搭建機械臂模型，使用Arduino單片機編寫程序代碼，實現(xiàn)機械臂模型控制。8.2展望挖掘機軌跡跟蹤控制系統(tǒng)的仿真，為實現(xiàn)自動高精度挖掘打下了理論基礎(chǔ)。本文采用SOLIDWORKS與MATLAB聯(lián)合仿真的策略實現(xiàn)了挖掘機工作裝置末端軌跡的顯示與控制，為現(xiàn)有的仿真方案提供了新的解決方案和策略。但同時本設(shè)計也存在著一些問題，例如：（1）本聯(lián)合仿真的方案的PID控制器采用的是MATLAB自帶的自動調(diào)參功能實現(xiàn)控制效果的優(yōu)化，并未采用其他文獻(xiàn)中的高級算法實現(xiàn)PID參數(shù)優(yōu)化功能，離實際應(yīng)用還有一定差距。（2）在制作機械臂模型時，本系統(tǒng)實現(xiàn)了通過串口監(jiān)視器輸入指令控制機械臂完成指定動作，但編寫控制程序仍有較大優(yōu)化空間，比如未來可以接入藍(lán)牙模塊，實現(xiàn)手機控制機械臂運動。以上存在的問題，還需要在未來的研究中進(jìn)一步解決。參考文獻(xiàn)[1]張鐵,朱明才.工程建設(shè)機械機電液一體化[M].東營:石油大學(xué)出版社,2001.[2]白玉梅,高紅斌,宋曉美.液壓挖掘機工作裝置的建模與運動學(xué)仿真[J].機械工程與自動化,2020(01):35-36.[3]張大慶,何清華,郝鵬等.液壓挖掘機鏟斗的軌跡跟蹤控制[J].吉林大學(xué)學(xué)報(工學(xué)版),2005(05):490-494.[4]周波,周輝.挖掘機器人工作軌跡跟蹤智能控制[J].武漢科