專(zhuān)利名稱(chēng)：用于噴藥移動(dòng)機(jī)器人的軌跡跟蹤滑?？刂葡到y(tǒng)和控制方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種移動(dòng)機(jī)器人的軌跡跟蹤控制，尤其涉及針對(duì)溫室環(huán)境下兩輪驅(qū)動(dòng) 噴藥移動(dòng)機(jī)器人軌跡跟蹤控制的滑?？刂破鞯臉?gòu)建方法。
背景技術(shù)：
上世紀(jì)70年代以來(lái)，中國(guó)已逐步推廣塑料大棚種植技術(shù)，并取得顯著的經(jīng)濟(jì)和社 會(huì)效益，如今，中國(guó)已成為最大的農(nóng)作物溫室生產(chǎn)國(guó)家。然而，我國(guó)的溫室生產(chǎn)管理水平與 自動(dòng)化程度與先進(jìn)國(guó)家相比仍存在一定的差距。在溫室生產(chǎn)設(shè)備方面，仍然用手動(dòng)噴霧器 進(jìn)行農(nóng)作物施藥。為有效的提高農(nóng)藥利用率，減少勞動(dòng)強(qiáng)度，并降低農(nóng)藥對(duì)作業(yè)人員的傷 害，有必要提高我國(guó)溫室生產(chǎn)設(shè)備的自動(dòng)化水平，以滿(mǎn)足現(xiàn)代精準(zhǔn)農(nóng)業(yè)生產(chǎn)的要求。
噴藥機(jī)器人是非常重要的現(xiàn)代溫室管理設(shè)備，越來(lái)越多的國(guó)家已經(jīng)在農(nóng)業(yè)生產(chǎn)中 使用溫室噴藥機(jī)器人，相關(guān)的技術(shù)水平也在不斷提高。然而，溫室內(nèi)作物種植密集，地面障 礙與空間障礙并存，為實(shí)現(xiàn)噴藥移動(dòng)機(jī)器人在這種非結(jié)構(gòu)化環(huán)境下對(duì)作物實(shí)施精準(zhǔn)噴藥， 準(zhǔn)確的軌跡跟蹤控制是噴藥機(jī)器人實(shí)現(xiàn)農(nóng)作物精準(zhǔn)噴藥的關(guān)鍵。
文 獻(xiàn)《Image-based Trajectory Tracking with Fuzzy Control for Nonholonomic MobileRobots(Tatsuya Kato, et. al, IECON 2011-37th Annual Conference on IEEE Industrial ElectronicsSociety, 2011, pp. 3299-3304.)引入一種通過(guò)基于圖像 處理的模糊控制器實(shí)現(xiàn)移動(dòng)機(jī)器人的軌跡跟蹤控制。首先建立獨(dú)立于機(jī)器人的圖像坐標(biāo) 系，將機(jī)器人攝像頭的信息映射到圖像坐標(biāo)系，通過(guò)控制圖像平面的信息間接控制機(jī)器人 狀態(tài)。通過(guò)圖形處理計(jì)算圖像坐標(biāo)系下的目標(biāo)軌跡的梯度和截距，從中獲得期望軌跡目標(biāo) 函數(shù)。設(shè)計(jì)一個(gè)模糊控制器對(duì)機(jī)器人的航向角和速度進(jìn)行控制，該模糊控制器的為61-20 結(jié)構(gòu)，輸入量為基于圖像處理的圖像坐標(biāo)系下的梯度和截距、梯度和截距的導(dǎo)數(shù)、航向角、 前進(jìn)速度)，輸出量為航向角和前進(jìn)速度的調(diào)節(jié)量。該控制器可以通過(guò)控制圖像平面的信 息間接控制機(jī)器人狀態(tài)。雖然目前圖像處理的軟硬件技術(shù)發(fā)展迅猛，但可靠的視覺(jué)識(shí)別算 法的計(jì)算耗時(shí)依然不可忽視，且溫室內(nèi)的作業(yè)環(huán)境有時(shí)要比田間的作業(yè)環(huán)境更差一些，如 室內(nèi)空間和地面設(shè)施密集、作物的密集度比田間大、室內(nèi)光線不夠充足等因素又增加了識(shí) 別難度，以至于視覺(jué)系統(tǒng)無(wú)法及時(shí)地感知機(jī)器人位姿變化；移動(dòng)機(jī)器人前進(jìn)和轉(zhuǎn)彎時(shí)，不可 避免地會(huì)與地面發(fā)生滑動(dòng)摩擦，使得系統(tǒng)的輸出控制量不能準(zhǔn)確反應(yīng)到控制系統(tǒng)中，從而 直接影響機(jī)器人軌跡跟蹤控制的性能；另外，文中采用模糊控制器對(duì)移動(dòng)機(jī)器人進(jìn)行軌跡 跟蹤控制，而模糊邏輯需要完備的人類(lèi)經(jīng)驗(yàn)，由于輸入量較多，其推理規(guī)則和模糊表急劇增 加；文獻(xiàn)《提高輪式移動(dòng)機(jī)器人性能的AKF和滑模相結(jié)合控制方法》(曹政才等，控制與決 策，2011年第26卷10期，第1409-1503頁(yè))針對(duì)輪式移動(dòng)機(jī)器人在實(shí)際工作中不可避免 地受到環(huán)境因素影響的問(wèn)題，采用Sage-Husa自適應(yīng)卡爾曼濾波對(duì)帶有白噪聲的參考軌跡 進(jìn)行估計(jì)，以提高測(cè)量信息的真實(shí)性；滑模算法用來(lái)控制機(jī)器人的速度和轉(zhuǎn)向，實(shí)現(xiàn)機(jī)器 人的跟蹤控制且抑制外界干擾。利用Backstepping方法選擇用于跟蹤的速度控制律，選擇 PI型滑模面，借助機(jī)器人動(dòng)力學(xué)模型確定等效控制量，當(dāng)系統(tǒng)有外來(lái)干擾時(shí)，考慮切換控制量。此外，引入變速函數(shù)VS (S，ε)來(lái)代替Sgn(S)削弱抖動(dòng)現(xiàn)象。該方法的主要問(wèn)題在于控制器的結(jié)構(gòu)和設(shè)計(jì)過(guò)程復(fù)雜，而且要求機(jī)器人能夠提供充分大的加速度，難于滿(mǎn)足溫室噴藥移動(dòng)機(jī)器人控制的實(shí)時(shí)性要求。
申請(qǐng)?zhí)柺荂N201110169879. 9，名稱(chēng)為“輪式移動(dòng)機(jī)器人的變結(jié)構(gòu)控制方法”的專(zhuān)利申請(qǐng)公開(kāi)了一種輪式移動(dòng)機(jī)器人的變結(jié)構(gòu)控制方法，采用多模態(tài)PID控制方法來(lái)控制機(jī)器人的直線移動(dòng)；采用PID控制與規(guī)則控制相結(jié)合的控制方法來(lái)校正小車(chē)的前進(jìn)方向；這兩種控制方式通過(guò)方向角和中心偏移量的變化進(jìn)行轉(zhuǎn)換，機(jī)器人的轉(zhuǎn)彎控制通過(guò)轉(zhuǎn)彎方向信息與位置信息來(lái)控制機(jī)器人以原地直角轉(zhuǎn)彎方式進(jìn)行轉(zhuǎn)彎，針對(duì)機(jī)器人所處的不同狀態(tài)采用不同的控制算法以及相應(yīng)的控制參數(shù)，較傳統(tǒng)的PID控制方法，具有一定的智能性。但該發(fā)明本質(zhì)上還是一種PID控制算法，由于溫室噴藥移動(dòng)機(jī)器人是集作物待噴面積、病蟲(chóng)害程度、距離信息、行走速度、噴藥量、藥液濃度等多個(gè)變量與一體的復(fù)雜系統(tǒng)，且各變量相互影響。此外，溫室環(huán)境中溫度、濕度、光照度、地面平整度、驅(qū)動(dòng)輪制造誤差等因素以及某些作物、溫室骨架、管道和道路附屬物等干擾的存在，使得上述PID控制方法難以控制機(jī)器人沿著規(guī)劃的路徑行駛，實(shí)際軌跡易偏離理想作業(yè)路線，因此在作業(yè)區(qū)域會(huì)產(chǎn)生較大重噴和漏噴區(qū)域。
另外，查閱國(guó)內(nèi)在輪式移動(dòng)機(jī)器人控制方面的專(zhuān)利情況可見(jiàn)，移動(dòng)機(jī)器人控制專(zhuān)利大都是控制系統(tǒng)結(jié)構(gòu)設(shè)計(jì)，由于機(jī)構(gòu)本體的制造、裝配、磨損等各種原因以及復(fù)雜的非結(jié)構(gòu)化農(nóng)田場(chǎng)景對(duì)傳感器信息采集的精度影響，勢(shì)必會(huì)影響溫室移動(dòng)機(jī)器人的軌跡跟蹤精度。
從上述溫室移動(dòng)機(jī)器人軌跡跟蹤控制相關(guān)技術(shù)可以看出，已有研究取得了一些成果，但是存在一定的局限性。模糊控制方法缺乏自學(xué)習(xí)能力，在移動(dòng)機(jī)器人軌跡跟蹤控制系統(tǒng)中不能在線調(diào)整模糊控制規(guī)則，且在輸入量輸出量的隸屬函數(shù)選擇上帶有一定主觀性，自適應(yīng)能力有限，難以獲得理想的軌跡跟蹤效果，另外，模糊規(guī)則依賴(lài)于操作人員的經(jīng)驗(yàn)，對(duì)于具有多輸入輸出量的溫室噴藥移動(dòng)機(jī)器人控制系統(tǒng)，其推理規(guī)則 和模糊控制表較復(fù)雜；PID控制方法只對(duì)線性系統(tǒng)性或簡(jiǎn)單的非線性系統(tǒng)適用，對(duì)于多變量、高度非線性且不確定因素及干擾并存的復(fù)雜溫室噴藥移動(dòng)機(jī)器人系統(tǒng)，其控制效果并不理想；而基于 Backstepping方法的滑?？刂频闹饕獑?wèn)題在于控制器的結(jié)構(gòu)和設(shè)計(jì)過(guò)程復(fù)雜，而且要求機(jī)器人能夠提供充分大的加速度，難于滿(mǎn)足溫室噴藥移動(dòng)機(jī)器人控制的實(shí)時(shí)性要求。發(fā)明內(nèi)容
本發(fā)明的目的是為克服上述現(xiàn)有技術(shù)的不足，針對(duì)溫室噴藥移動(dòng)機(jī)器人，提出一種基于積分加權(quán)增益趨近律的滑?？刂品椒▽?shí)現(xiàn)其軌跡跟蹤控制，該控制算法增益項(xiàng)中包含切換函數(shù)s積分的絕對(duì)值，當(dāng)s趨近于零時(shí)，切換項(xiàng)的增益趨近于零，從而消除抖振；當(dāng)系統(tǒng)狀態(tài)不在滑動(dòng)模態(tài)時(shí)，雖然S值較大，由于積分加權(quán)系數(shù)kf為負(fù)，可有效避免當(dāng)系統(tǒng)不在滑動(dòng)模態(tài)階段時(shí)切換增益的增大。本發(fā)明所設(shè)計(jì)滑?？刂破鞑灰蕾?lài)于被控對(duì)象精確的數(shù)學(xué)模型，具有響應(yīng)快、對(duì)參數(shù)和環(huán)境變化不敏感、無(wú)需系統(tǒng)在線辨識(shí)、物理實(shí)現(xiàn)簡(jiǎn)單等優(yōu)點(diǎn)， 而積分加權(quán)增益趨近律的引入可有效消除滑?？刂葡到y(tǒng)固有的抖振問(wèn)題。在不增加系統(tǒng)硬件成本的條件下，通過(guò)軟件控制的方法提高移動(dòng)機(jī)器人的軌跡跟蹤精度和抗干擾能力，從而進(jìn)一步實(shí)現(xiàn)溫室移動(dòng)機(jī)器人在行間對(duì)作物實(shí)施精準(zhǔn)噴藥。
本發(fā)明采用的技術(shù)方案是，用于噴藥移動(dòng)機(jī)器人的軌跡跟蹤滑?？刂葡到y(tǒng)，由車(chē)體位姿與驅(qū)動(dòng)輪速度映射模塊、左輪控制系統(tǒng)和右輪控制系統(tǒng)組成；
所述車(chē)體位姿與驅(qū)動(dòng)輪速度映射模塊，用于根據(jù)帶有電機(jī)驅(qū)動(dòng)軸干擾項(xiàng)的輪式移動(dòng)機(jī)器人各驅(qū)動(dòng)輪控制器的被控對(duì)象數(shù)學(xué)模型和計(jì)算機(jī)視覺(jué)系統(tǒng)識(shí)別出的行走路徑，確定在實(shí)現(xiàn)噴藥移動(dòng)機(jī)器人路徑跟蹤過(guò)程中各支路驅(qū)動(dòng)電機(jī)的期望速度軌跡《d，分別輸入左輪控制系統(tǒng)和右輪控制系統(tǒng)；
所述左輪控制系統(tǒng)包括滑?？刂破?、驅(qū)動(dòng)器、直流電機(jī)、齒輪箱和左輪，所述直流電機(jī)的輸出端和所述滑?？刂破鞯妮斎攵酥g還設(shè)有編碼器；所述滑模控制器將控制量作為驅(qū)動(dòng)指令發(fā)送給驅(qū)動(dòng)器，驅(qū)動(dòng)器通過(guò)調(diào)節(jié)PWM信號(hào)的不同占空比來(lái)調(diào)節(jié)直流電機(jī)速度， 直流電機(jī)將軸的旋轉(zhuǎn)運(yùn)動(dòng)輸入到齒輪箱，齒輪箱的輸出軸驅(qū)動(dòng)機(jī)器人左輪轉(zhuǎn)動(dòng)，所述編碼器采用測(cè)頻法對(duì)電機(jī)的轉(zhuǎn)速進(jìn)行檢測(cè)，考慮驅(qū)動(dòng)輪滑轉(zhuǎn)的情況下，計(jì)算移動(dòng)機(jī)器人左右驅(qū)動(dòng)電動(dòng)機(jī)的實(shí)際運(yùn)動(dòng)角速度ω，實(shí)際運(yùn)動(dòng)角加速度m，并計(jì)算各驅(qū)動(dòng)電動(dòng)機(jī)期望角速度與實(shí)際角速度的偏差e和偏差導(dǎo)數(shù) 輸入至所述滑?？刂破?；所述滑?？刂破鳎鶕?jù)偏差e、偏差導(dǎo)數(shù) 和滑模面參數(shù)獲得滑?？刂破髑袚Q函數(shù)S，再根據(jù)滑?？刂破髑袚Q函數(shù)s和被控對(duì)象數(shù)學(xué)模型，確定移動(dòng)機(jī)器人驅(qū)動(dòng)電動(dòng)機(jī)的控制量；
所述右輪控制系統(tǒng)的結(jié)構(gòu)和左輪控制系統(tǒng)的結(jié)構(gòu)相同。
作為本發(fā)明的進(jìn)一步改進(jìn)，所述驅(qū)動(dòng)電動(dòng)機(jī)的控制量根據(jù)下式確定
權(quán)利要求
1.用于噴藥移動(dòng)機(jī)器人的軌跡跟蹤滑?？刂品椒?，其特征是，該方法采用如下步驟 1)對(duì)移動(dòng)機(jī)器人進(jìn)行機(jī)構(gòu)分析，建立具有非完整性約束的移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型； 2)考慮溫室移動(dòng)機(jī)器人行駛要求和結(jié)構(gòu)特點(diǎn)及與之作用的土壤溫室環(huán)境因素，建立帶有電機(jī)驅(qū)動(dòng)軸干擾項(xiàng)的輪式移動(dòng)機(jī)器人各支路控制器的被控對(duì)象數(shù)學(xué)模型； 3)利用計(jì)算機(jī)視覺(jué)系統(tǒng)識(shí)別出溫室農(nóng)作物行或壟等區(qū)域的邊界作為行走路徑，根據(jù)步驟I)所推導(dǎo)的運(yùn)動(dòng)學(xué)模型，確定在實(shí)現(xiàn)噴藥移動(dòng)機(jī)器人路徑跟蹤過(guò)程中各支路驅(qū)動(dòng)電機(jī)的期望運(yùn)動(dòng)軌跡； 4)利用增量式光電編碼器，采用測(cè)頻法對(duì)電機(jī)的轉(zhuǎn)速進(jìn)行檢測(cè)，在考慮驅(qū)動(dòng)輪滑轉(zhuǎn)的情況下，計(jì)算移動(dòng)機(jī)器人左右驅(qū)動(dòng)電動(dòng)機(jī)的實(shí)際運(yùn)動(dòng)角速度ω，實(shí)際運(yùn)動(dòng)角加速度 ,并計(jì)算各驅(qū)動(dòng)電動(dòng)機(jī)期望角速度與實(shí)際角速度的偏差e和偏差導(dǎo)數(shù) ; 5)考慮移動(dòng)機(jī)器人驅(qū)動(dòng)電機(jī)特性及速度要求，選擇合適的滑模面參數(shù)，構(gòu)建滿(mǎn)足驅(qū)動(dòng)電機(jī)速度控制要求的滑模切換函數(shù)； 6)在確定滑模面函數(shù)s基礎(chǔ)上，設(shè)計(jì)加權(quán)積分增益趨近律確定移動(dòng)機(jī)器人左右驅(qū)動(dòng)電動(dòng)機(jī)的滑?？刂破骺刂屏浚? 7)將移動(dòng)機(jī)器人電機(jī)驅(qū)動(dòng)控制量分別發(fā)送給左右電機(jī)驅(qū)動(dòng)器，實(shí)現(xiàn)噴藥移動(dòng)機(jī)器人溫室環(huán)境下的軌跡跟蹤。
2.根據(jù)權(quán)利要求1所述的?？刂品椒?，其特征是 步驟4)中，考慮溫室地面和驅(qū)動(dòng)輪滑轉(zhuǎn)的情況，實(shí)際的滑轉(zhuǎn)率與設(shè)置的理論速度，負(fù)載大小以及地面情況有關(guān)，其與理論速度近似成線性關(guān)系為
3.根據(jù)權(quán)利要求1所述的控制方法，其特征是 所述步驟6)中加權(quán)積分增益趨近律為 V = -ks — k w |p| Sgn(-v)P = {k,p + s)dl, /: / <() 式中，s為滑?？刂魄袚Q函數(shù)，P為加權(quán)積分增益項(xiàng)，sgn(s)是關(guān)于滑模面s的符號(hào)函數(shù)，k和 <是可設(shè)定的加權(quán)積分增益趨近律參數(shù)，在積分項(xiàng)P的表達(dá)式中，kf是負(fù)的加權(quán)系數(shù)，當(dāng) P >0 時(shí)，kf P〈O，當(dāng) P〈O 時(shí)，kf P >0 ; 所述移動(dòng)機(jī)器人驅(qū)動(dòng)電動(dòng)機(jī)的控制量根據(jù)下式確定
4.用于噴藥移動(dòng)機(jī)器人的軌跡跟蹤滑?？刂葡到y(tǒng)，由車(chē)體位姿與驅(qū)動(dòng)輪速度映射模塊、左輪控制系統(tǒng)和右輪控制系統(tǒng)組成；其特征是， 所述車(chē)體位姿與驅(qū)動(dòng)輪速度映射模塊，用于根據(jù)帶有電機(jī)驅(qū)動(dòng)軸干擾項(xiàng)的輪式移動(dòng)機(jī)器人各驅(qū)動(dòng)輪控制器的被控對(duì)象數(shù)學(xué)模型和計(jì)算機(jī)視覺(jué)系統(tǒng)識(shí)別出的行走路徑，確定在實(shí)現(xiàn)噴藥移動(dòng)機(jī)器人路徑跟蹤過(guò)程中各支路驅(qū)動(dòng)電機(jī)的期望速度軌跡，分別輸入左輪控制系統(tǒng)和右輪控制系統(tǒng)； 所述左輪控制系統(tǒng)包括滑?？刂破?、驅(qū)動(dòng)器、直流電機(jī)、齒輪箱和左輪，所述直流電機(jī)的輸出端和所述滑?？刂破鞯妮斎攵酥g還設(shè)有編碼器；所述滑模控制器將控制量作為驅(qū)動(dòng)指令發(fā)送給驅(qū)動(dòng)器，驅(qū)動(dòng)器通過(guò)調(diào)節(jié)PWM信號(hào)的不同占空比來(lái)調(diào)節(jié)直流電機(jī)速度，直流電機(jī)將軸的旋轉(zhuǎn)運(yùn)動(dòng)輸入到齒輪箱，齒輪箱的輸出軸驅(qū)動(dòng)機(jī)器人左輪轉(zhuǎn)動(dòng)，所述編碼器采用測(cè)頻法對(duì)電機(jī)的轉(zhuǎn)速進(jìn)行檢測(cè)，考慮驅(qū)動(dòng)輪滑轉(zhuǎn)的情況下，計(jì)算移動(dòng)機(jī)器人左右驅(qū)動(dòng)電動(dòng)機(jī)的實(shí)際運(yùn)動(dòng)角速度ω，實(shí)際運(yùn)動(dòng)角加速度0，并計(jì)算各驅(qū)動(dòng)電動(dòng)機(jī)期望角速度與實(shí)際角速度的偏差e和偏差導(dǎo)數(shù) 輸入至所述滑?？刂破?；所述滑?？刂破鳎鶕?jù)偏差e、偏差導(dǎo)數(shù) 和滑模面參數(shù)獲得滑?？刂破髑袚Q函數(shù)S，再根據(jù)滑?？刂破髑袚Q函數(shù)s和被控對(duì)象數(shù)學(xué)模型，確定移動(dòng)機(jī)器人驅(qū)動(dòng)電動(dòng)機(jī)的控制量； 所述右輪控制系統(tǒng)的結(jié)構(gòu)和左輪控制系統(tǒng)的結(jié)構(gòu)相同。
5.根據(jù)權(quán)利要求4所述的軌跡跟蹤滑?？刂葡到y(tǒng)，其特征是， 所述驅(qū)動(dòng)電動(dòng)機(jī)的控制量根據(jù)下式確定
全文摘要
用于噴藥移動(dòng)機(jī)器人的軌跡跟蹤滑?？刂品椒ǎ捎萌缦虏襟E對(duì)移動(dòng)機(jī)器人進(jìn)行機(jī)構(gòu)分析，建立具有非完整性約束的移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型；建立帶有電機(jī)驅(qū)動(dòng)軸干擾項(xiàng)的輪式移動(dòng)機(jī)器人各支路控制器的被控對(duì)象數(shù)學(xué)模型；利用計(jì)算機(jī)視覺(jué)系統(tǒng)識(shí)別行走路徑，根據(jù)上步驟所推導(dǎo)的運(yùn)動(dòng)學(xué)模型確定各支路驅(qū)動(dòng)電機(jī)的期望運(yùn)動(dòng)軌跡；對(duì)電機(jī)的轉(zhuǎn)速進(jìn)行檢測(cè)，計(jì)算移動(dòng)機(jī)器人左右驅(qū)動(dòng)電動(dòng)機(jī)的實(shí)際運(yùn)動(dòng)角速度，實(shí)際運(yùn)動(dòng)角加速度，計(jì)算各驅(qū)動(dòng)電動(dòng)機(jī)期望角速度與實(shí)際角速度的偏差和偏差導(dǎo)數(shù)；構(gòu)建滿(mǎn)足驅(qū)動(dòng)電機(jī)速度控制要求的滑模切換函數(shù)；在確定滑模面函數(shù)s基礎(chǔ)上，確定移動(dòng)機(jī)器人左右驅(qū)動(dòng)電動(dòng)機(jī)的滑?？刂破骺刂屏?；將移動(dòng)機(jī)器人電機(jī)驅(qū)動(dòng)控制量分發(fā)送給左右電機(jī)驅(qū)動(dòng)器。
文檔編號(hào)G05D1/02GK103019239SQ20121049043
公開(kāi)日2013年4月3日 申請(qǐng)日期2012年11月27日 優(yōu)先權(quán)日2012年11月27日
發(fā)明者高國(guó)琴, 周海燕 申請(qǐng)人:江蘇大學(xué)