綠籬移動(dòng)智能機(jī)器人采用移動(dòng)車輛搭載四自由度機(jī)械手的結(jié)構(gòu)，機(jī)械手由一系列臂桿組成，在臂桿系的開(kāi)端，安裝有末端執(zhí)行件(旋切、抓取工具等)，由于關(guān)節(jié)的可控運(yùn)動(dòng)，通過(guò)調(diào)整臂桿在空間的相對(duì)位置，最終構(gòu)成了末端執(zhí)行機(jī)構(gòu)在空間的不同位置和方向。臂桿的運(yùn)動(dòng)是通過(guò)關(guān)節(jié)運(yùn)動(dòng)來(lái)實(shí)現(xiàn)的，機(jī)器人的關(guān)節(jié)按運(yùn)動(dòng)方式可分為兩類：一類是直線關(guān)節(jié)(P)，另一類是旋轉(zhuǎn)關(guān)節(jié)(R)，分別可以實(shí)現(xiàn)直線運(yùn)動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng)。依靠轉(zhuǎn)動(dòng)關(guān)節(jié)和直線關(guān)節(jié)的不同組合，構(gòu)成了不同類型的機(jī)器人手臂。運(yùn)動(dòng)的自由度越多，手臂的運(yùn)動(dòng)就越復(fù)雜。為了更好地確定移動(dòng)機(jī)器人執(zhí)行機(jī)構(gòu)的運(yùn)動(dòng)軌跡及位置、關(guān)節(jié)的受力、平衡和撓度狀況，必須對(duì)運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)進(jìn)行分析，求解運(yùn)動(dòng)學(xué)的正問(wèn)題和逆問(wèn)題。建立有關(guān)動(dòng)力學(xué)方程，以解決驅(qū)動(dòng)或驅(qū)動(dòng)扭矩和臂桿運(yùn)動(dòng)之間的關(guān)系，即動(dòng)力學(xué)的正問(wèn)題和逆問(wèn)題。基于移動(dòng)車輛和機(jī)械手單獨(dú)控制的思想， 只對(duì)機(jī)械手的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)單獨(dú)分析。

運(yùn)動(dòng)學(xué)

機(jī)器人運(yùn)動(dòng)學(xué)主要有以下兩個(gè)基本問(wèn)題：

(1)對(duì)一給定的機(jī)器人，己知桿件幾何參數(shù)和關(guān)節(jié)變量，求末段執(zhí)行器相對(duì)于給定坐標(biāo)系的位置和姿態(tài)。給定坐標(biāo)系為固定在大地上的笛卡爾坐標(biāo)系，作為機(jī)器人的總體坐標(biāo)系，也稱為世界坐標(biāo)系(World Coordinate)；

(2)已知機(jī)器人桿件的幾何參數(shù)，給定末端執(zhí)行器相對(duì)于總體坐標(biāo)系的位置和姿態(tài)，確定關(guān)節(jié)變量的大小。第一個(gè)問(wèn)題常稱作運(yùn)動(dòng)學(xué)正問(wèn)題(DKP-DirectKinematics Problems)第二個(gè)問(wèn)題常稱作運(yùn)動(dòng)學(xué)逆問(wèn)題(IKP．Inverse Kinematics Problems)。機(jī)器人手臂的關(guān)節(jié)變量是獨(dú)立變量，而末端執(zhí)行器的作業(yè)通常在世界坐標(biāo)中說(shuō)明。機(jī)器人運(yùn)動(dòng)學(xué)逆問(wèn)題是編制機(jī)器人運(yùn)動(dòng)控制系統(tǒng)軟件所必備的知識(shí)。

綠籬修剪移動(dòng)機(jī)器人執(zhí)行機(jī)構(gòu)的結(jié)構(gòu)及簡(jiǎn)化

綠籬修剪機(jī)移動(dòng)機(jī)器人的結(jié)構(gòu)主要出工作裝置、回轉(zhuǎn)裝置和行走裝置組成。工作裝置包括末端執(zhí)行器、大臂、小臂及相應(yīng)的三組伺服系統(tǒng)。它是由行走基座上轉(zhuǎn)臺(tái)的回轉(zhuǎn)關(guān)節(jié)，大臂相對(duì)轉(zhuǎn)臺(tái)的旋轉(zhuǎn)關(guān)節(jié)，小臂繞大臂末端的旋轉(zhuǎn)關(guān)節(jié)以及末端執(zhí)行器繞小臂末端的旋轉(zhuǎn)關(guān)節(jié)構(gòu)成的系統(tǒng)。該執(zhí)行機(jī)構(gòu)具有四個(gè)自由度(不包括行走運(yùn)動(dòng))，每個(gè)運(yùn)動(dòng)部件對(duì)應(yīng)著一個(gè)驅(qū)動(dòng)裝置，包括回轉(zhuǎn)驅(qū)動(dòng)裝置和三組伺服系統(tǒng)。

基于Denavit-Hartenberg(D—H)法的建模為描述相鄰桿件間的平移和轉(zhuǎn)動(dòng)關(guān)系，Denavit和Hartenberg(1955)提出了一種為關(guān)節(jié)鏈中的每一個(gè)桿件建立附體坐標(biāo)系的矩陣方法。剮性桿件的D．H法取決于桿件的四個(gè)幾何參數(shù)q，q，Z，諺。它為每個(gè)關(guān)節(jié)處的桿件坐標(biāo)系建立4×4齊次變換矩陣，表示它與前一桿件坐標(biāo)系的關(guān)系。這樣逐次變換，用“手部坐標(biāo)”表示的術(shù)端執(zhí)行器可被變換并用基座坐標(biāo)表示。一個(gè)4×4齊次變換矩陣是形如下面的形式：

研究中機(jī)械手由一串用轉(zhuǎn)動(dòng)或平移關(guān)節(jié)連接的剛體(桿件)組成。每一對(duì)關(guān)節(jié)一桿件構(gòu)成一個(gè)自由度。桿件的編號(hào)由手臂的固定基座開(kāi)始，固定基座可看成桿件0，第一個(gè)運(yùn)動(dòng)體是桿件l，依此類推，最后一個(gè)桿件與工具相連，關(guān)節(jié)I處于連接桿件1和機(jī)座之間，每個(gè)桿件至多與另外兩個(gè)桿件相聯(lián)，而不構(gòu)成閉環(huán)。任何桿件f都可用兩個(gè)尺度表征，桿件f的長(zhǎng)度珥，是桿件上兩個(gè)關(guān)節(jié)軸線的晟短距離；桿件f的扭轉(zhuǎn)角a．，是兩個(gè)關(guān)節(jié)軸線的夾角。通常，在每個(gè)關(guān)節(jié)軸線上連接有兩根桿件，每個(gè)桿件各有一根和軸線垂直的法線。兩個(gè)桿件的相對(duì)位置有兩桿件的距離諺(關(guān)節(jié)軸上兩個(gè)法線的距離)和夾角a(關(guān)節(jié)軸上兩個(gè)法線的夾角)確定。

n關(guān)節(jié)機(jī)器人需建立n 1個(gè)坐標(biāo)系，其中參考(機(jī)座)坐標(biāo)系位00K％Zo，機(jī)械手末端的坐標(biāo)系位q五K乙，第f關(guān)節(jié)上的坐標(biāo)系為q一。墨一，r。zf一．。確定和建立每個(gè)坐標(biāo)系應(yīng)根據(jù)下面3條規(guī)則：

①z軸沿著第f關(guān)節(jié)的運(yùn)動(dòng)軸；

②x，軸垂直于Zj一。軸并指向離開(kāi)互一。軸的方向：

③y軸按右手坐標(biāo)系的要求建立。

按照這些規(guī)則，第0號(hào)坐標(biāo)系在基座上的位置和方向可任選，只要乙軸沿著第1關(guān)節(jié)運(yùn)動(dòng)軸。根據(jù)實(shí)際情況和建立坐標(biāo)系的慣例，將第0號(hào)坐標(biāo)系建在車體。第n坐標(biāo)系可放在手的任何部位，只要x軸與z0軸垂直。

根據(jù)上述對(duì)桿件參數(shù)及坐標(biāo)系的定義，描述串聯(lián)機(jī)器人相鄰坐標(biāo)系之間的關(guān)節(jié)關(guān)系可歸結(jié)為如下4個(gè)參數(shù)：2：繞互一。軸(右手規(guī)則)由耳。軸向置軸的關(guān)節(jié)角i珥：從第i-1坐標(biāo)系的原點(diǎn)到Zf一，軸和五軸的交點(diǎn)沿互，。軸的距離：q：繞Z軸(右手規(guī)則)由ZJ一．軸轉(zhuǎn)向zf軸的偏角。

運(yùn)動(dòng)學(xué)正解所謂正運(yùn)動(dòng)學(xué)(direct kinematics，簡(jiǎn)稱DK)，就是當(dāng)機(jī)器人各關(guān)節(jié)的變量給定時(shí)，求解這些變位量在以機(jī)器人的指尖位置和方向?yàn)榛A(chǔ)的基準(zhǔn)坐標(biāo)系中，如何進(jìn)行表示的問(wèn)題。即在出關(guān)節(jié)的位置、速度、加速度時(shí)，求各桿件的位置、姿態(tài)、速度、角速度、加速度、角加速度的問(wèn)題，特別是求終端桿件及末端執(zhí)行器的位置、姿態(tài)、速度、角速度的問(wèn)題。作為正運(yùn)動(dòng)學(xué)的解析法，已知有齊次變換法和向量法。下面按照齊次變換法來(lái)進(jìn)行正運(yùn)動(dòng)學(xué)的解析。齊次坐標(biāo)變換是坐標(biāo)系‘對(duì)坐標(biāo)系‘一。的變換，是由繞z川旋轉(zhuǎn)0i角、再沿著旋轉(zhuǎn)后坐標(biāo)系的z。，x。軸分別作di，a，的平移及繞平移后坐標(biāo)系的x。軸旋轉(zhuǎn)口，角合成。

機(jī)械手動(dòng)力學(xué)

動(dòng)力學(xué)研究物體運(yùn)動(dòng)和受力之間的關(guān)系。機(jī)械手動(dòng)力學(xué)需要解決的問(wèn)題是：根據(jù)關(guān)節(jié)驅(qū)動(dòng)力或力，計(jì)算操作手的關(guān)節(jié)位移、速度、加速度(動(dòng)力學(xué)正解問(wèn)題)。己知機(jī)械手的關(guān)節(jié)位移、速度、加速度，求出所需的關(guān)節(jié)力矩或力(動(dòng)力學(xué)逆解問(wèn)題)。機(jī)械手動(dòng)力學(xué)方程是一組描述機(jī)械手動(dòng)態(tài)特性的數(shù)學(xué)方程，可用于計(jì)算機(jī)仿真、機(jī)器人結(jié)構(gòu)和運(yùn)動(dòng)設(shè)計(jì)的計(jì)算。機(jī)器人的實(shí)際動(dòng)力學(xué)模型可根據(jù)已知的物理定律求得，描述機(jī)械手動(dòng)力學(xué)方程有多種方法，比如：拉格朗日一歐拉方程，牛頓．歐拉方程，以及達(dá)朗貝爾方程，凱恩(Kane)方法、旋量對(duì)偶數(shù)方法等等”在研究機(jī)械手的動(dòng)力學(xué)方程時(shí)，假設(shè)研究的對(duì)象是剛體，且電氣控制器件的動(dòng)態(tài)特性、回差和齒輪摩擦都予以忽略。本課題研制的綠籬修剪機(jī)器人主要是用來(lái)進(jìn)行高速公路中央隔離帶綠籬修剪，在綠籬修剪作業(yè)時(shí)機(jī)械手末端執(zhí)行器(旋轉(zhuǎn)刀頭)回轉(zhuǎn)中心線必須和作業(yè)面垂直，機(jī)械手可以簡(jiǎn)化成一個(gè)三自由度的平面機(jī)器人，在XZ平面上跟蹤綠籬的幾何中心，建立綠籬修剪機(jī)器人的動(dòng)力學(xué)模型。本研究采用拉格朗日一歐拉方程進(jìn)行動(dòng)力學(xué)方程求解。機(jī)械手動(dòng)力學(xué)方程中組成來(lái)看，各關(guān)節(jié)的驅(qū)動(dòng)扭矩是由慣量載荷，關(guān)節(jié)之間的反作用力(科氏力、離心力)以及重力載荷等構(gòu)成，而且它們的非線性程度和運(yùn)動(dòng)坐標(biāo)間的關(guān)系復(fù)雜。關(guān)節(jié)越多，參與的運(yùn)動(dòng)坐標(biāo)數(shù)越多，由各慣量項(xiàng)系數(shù)表示的組成多項(xiàng)式的成分也更復(fù)雜。針對(duì)這種情況，在控制設(shè)計(jì)中，常常不得不作一些必要的簡(jiǎn)化，比如將一些慣量項(xiàng)加以忽略，或使桿件的運(yùn)動(dòng)控制在縱向與橫向平面內(nèi)運(yùn)動(dòng)，從而把多自由度的復(fù)雜動(dòng)力學(xué)系統(tǒng)簡(jiǎn)化成簡(jiǎn)單動(dòng)力學(xué)系統(tǒng)來(lái)處理。但這樣做的結(jié)果是控制運(yùn)算的速度提高了，控制精度下降了，也就是機(jī)械手臂端的實(shí)際運(yùn)動(dòng)軌跡和編程軌跡之間的誤差及過(guò)調(diào)現(xiàn)象增加。但對(duì)綠籬修剪來(lái)說(shuō)，這種精度誤差是可以接受的。

根據(jù)機(jī)器人動(dòng)力學(xué)方程，將有關(guān)機(jī)器人的結(jié)構(gòu)參數(shù)(桿件長(zhǎng)度、質(zhì)量、附加重量、旋轉(zhuǎn)刀頭自重等)代入公式計(jì)算即可得出各桿件的輸出扭矩，從而確定出各關(guān)節(jié)的電機(jī)輸出功率，作為系統(tǒng)設(shè)計(jì)的重要參考依據(jù)。

基于PID和FCMAC的位置伺服控制

機(jī)械手的控制方式依據(jù)路徑約束和障礙約束可分為如表4．2的四種方式，本文機(jī)械手控制屬于路徑無(wú)約束控制，為了簡(jiǎn)化控制方法，使用位置控制。機(jī)械手在跟蹤路徑時(shí)工作范圍較小，運(yùn)動(dòng)控制采用關(guān)節(jié)伺服控制。移動(dòng)車輛的動(dòng)力學(xué)和機(jī)械手的動(dòng)力學(xué)存在耦合，將耦合視為負(fù)載擾動(dòng)。常規(guī)的位置伺服系統(tǒng)一般用PID控制算法， PID控制具有結(jié)構(gòu)簡(jiǎn)單、穩(wěn)定性好、易于工程實(shí)現(xiàn)等憂點(diǎn)，魯棒性較差，對(duì)于高性能、高精度的位置伺服系統(tǒng)，PID控制有時(shí)難以滿足要求。人們提出了改進(jìn)的方法，提出了采用模糊控制方法進(jìn)行位置伺服，采用了PIDjJ[I智能控制方法，尤其是神經(jīng)網(wǎng)絡(luò)的智能控制對(duì)參數(shù)不確定和非線性有獨(dú)特的優(yōu)點(diǎn)，可實(shí)現(xiàn)動(dòng)態(tài)補(bǔ)償和動(dòng)態(tài)調(diào)整PID的參數(shù)，本研究采用FCMAC實(shí)現(xiàn)前饋控制補(bǔ)償不完整建模和耦合干擾.

Albus基于神經(jīng)生理學(xué)于1975年提出了一種小腦模型關(guān)節(jié)控制器(CMAC)，它通過(guò)局部逼近方法來(lái)學(xué)習(xí)復(fù)雜的非線性函數(shù)映射，已被廣泛地應(yīng)用于函數(shù)逼近、模式識(shí)別和機(jī)器人控制等。CMAC神經(jīng)元網(wǎng)絡(luò)的優(yōu)點(diǎn)是采用局部逼近的方法，每次修改的權(quán)很少，所以學(xué)習(xí)速度很快，使CMAC可用于實(shí)時(shí)控制。CMAC本質(zhì)上是用階梯函數(shù)去擬合一個(gè)光滑曲面，因此為了提高分辨和泛化能力，勢(shì)必要增加存貯容量。同時(shí)存貯容量也隨輸入維數(shù)的增加而呈指數(shù)增長(zhǎng)，因此泛化能力與存貯容量之間的矛盾是CMAC的一個(gè)主要問(wèn)題。一般來(lái)說(shuō)，神經(jīng)網(wǎng)絡(luò)的內(nèi)部知識(shí)表達(dá)方式是不清楚的，但是它具有學(xué)習(xí)能力，神經(jīng)網(wǎng)絡(luò)每次學(xué)習(xí)時(shí)，只能從任意初始值開(kāi)始，不能利用必要的初始經(jīng)驗(yàn)和知識(shí)，收斂速度較慢，容易陷入局部極值。模糊邏輯擅長(zhǎng)表達(dá)近似或定性的知識(shí)，但是無(wú)學(xué)習(xí)能力，隸屬函數(shù)和模糊規(guī)則只能主觀或者是用實(shí)驗(yàn)的方法來(lái)進(jìn)行選擇，不能依據(jù)積累的經(jīng)驗(yàn)自動(dòng)改善系統(tǒng)性能。

將模糊邏輯引入CMAC(小腦模型關(guān)節(jié)控制器)后，F(xiàn)CMAC是一種將模糊邏輯和CMAC神經(jīng)網(wǎng)絡(luò)相結(jié)合的神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)，可以有效的解決泛化能力和存儲(chǔ)容量間的矛盾，且收斂速度比CMAC快，精度高“”，使得FCMAC結(jié)構(gòu)神經(jīng)網(wǎng)絡(luò)特別適合于實(shí)時(shí)應(yīng)用的場(chǎng)合。考慮到綠籬修剪對(duì)實(shí)時(shí)性有很高的要求，我們?cè)O(shè)計(jì)了如圖4．3的FCMAC模糊神經(jīng)網(wǎng)絡(luò)。

網(wǎng)絡(luò)由如下的5層所組成：

(1)輸入層：輸入空間維一維變量x=五。

(2)模糊層：計(jì)算隸屬度函數(shù)，該層完成輸入變量的模糊化，相當(dāng)于CMAC的感知層(Sensor layer)，相當(dāng)于模糊邏輯控制器的fuzzifier。隸屬函數(shù)一般取高斯型隸屬函數(shù)：

在PID調(diào)節(jié)系統(tǒng)中，比例調(diào)節(jié)是一種最簡(jiǎn)單的調(diào)節(jié)方式，它具有反應(yīng)快、無(wú)滯后的特點(diǎn)，能及時(shí)克服擾動(dòng)干擾，是被控參數(shù)穩(wěn)定在給定值附近。但比例控制不能消除穩(wěn)態(tài)誤差，當(dāng)K。過(guò)大時(shí)會(huì)引起系統(tǒng)不穩(wěn)定。積分控制的作用時(shí)，只要系統(tǒng)存在偏差，積分控制作用就不斷的積累，輸出控制量以消除穩(wěn)態(tài)誤差，因此，只要有足夠的時(shí)間，積分作用將能完全消除誤差，但積分調(diào)節(jié)動(dòng)作緩慢，其調(diào)節(jié)作用總是滯后于偏差信號(hào)的變化。而且，當(dāng)積分作用太強(qiáng)時(shí)，會(huì)使系統(tǒng)超調(diào)過(guò)大，甚至使系統(tǒng)出現(xiàn)振蕩。微分控制可以減小超調(diào)量，使系統(tǒng)穩(wěn)定性提高。它能反映偏差信號(hào)的變化趨勢(shì)，并能在偏差信號(hào)變得不很大以前，在系統(tǒng)中引入一個(gè)有效超前修正信號(hào)，從而加快系統(tǒng)的動(dòng)作速度，減小調(diào)節(jié)時(shí)間。在實(shí)際應(yīng)用中，PID調(diào)節(jié)器的實(shí)現(xiàn)分模擬和數(shù)字兩方法。模擬法就是利用硬件電路實(shí)現(xiàn)PID調(diào)節(jié)規(guī)律。數(shù)字法就是對(duì)經(jīng)典的模擬PID進(jìn)行了數(shù)字模擬，用數(shù)字調(diào)節(jié)來(lái)代替模擬調(diào)節(jié)器。在采樣周期較小時(shí)，數(shù)字模擬P1D控制算法是一種較理想的控制算法。神經(jīng)網(wǎng)絡(luò)在機(jī)器人控制中的應(yīng)用，目前有多種控制形式，如神經(jīng)網(wǎng)絡(luò)直接逆運(yùn)動(dòng)控制、神經(jīng)網(wǎng)絡(luò)前饋控制，神經(jīng)網(wǎng)絡(luò)反饋控制等等，本課題采用的是FCMAC神經(jīng)網(wǎng)絡(luò)前饋控制。FCMAC與PID復(fù)合控制結(jié)構(gòu)如圖4-4所示，該系統(tǒng)通過(guò)FCMAC和PID的復(fù)合控制實(shí)現(xiàn)前饋控制，其特點(diǎn)是為：

(1)模糊小腦神經(jīng)網(wǎng)絡(luò)實(shí)現(xiàn)前饋控制，實(shí)現(xiàn)被控制對(duì)象的逆動(dòng)態(tài)模型

(2)PID控制器實(shí)現(xiàn)反饋控制， 保證系統(tǒng)的穩(wěn)定性，且抑制擾動(dòng)。

交流伺服系統(tǒng)的模型

高精度的位置控制伺服系統(tǒng)通常選用交流永磁同步電動(dòng)機(jī)作為其執(zhí)行元件，其中，電機(jī)伺服系統(tǒng)主要由功率逆變、PWM速度控制、電流控制電路、電機(jī)、以及角位移傳感器構(gòu)成。通常位置控制環(huán)作為系統(tǒng)外環(huán)，而伺服驅(qū)動(dòng)單元作為系統(tǒng)內(nèi)環(huán)。對(duì)伺服系統(tǒng)進(jìn)行有效的控制三相永磁同步伺服電動(dòng)機(jī)(PMSM)及其驅(qū)動(dòng)器的數(shù)學(xué)模型如圖。

本章對(duì)機(jī)械手的動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)進(jìn)行了分干斤，得到了運(yùn)動(dòng)學(xué)的正逆解和動(dòng)力學(xué)方程：采用機(jī)械手和移動(dòng)體分開(kāi)控制的思想，將機(jī)械手的耦合視為擾動(dòng)，為了斃暇PID控涮豹缺點(diǎn)，擒逡了FCMAC與PID籀縫合熬翦饋控制系統(tǒng)，謗湊結(jié)果誠(chéng)明FCMAC與PID結(jié)合豹控翻效祭鐃予PID舞法。