某溫室采摘機(jī)器人的運(yùn)動(dòng)控制系統(tǒng)總體方案目錄TOC\o"1-3"\h\u19170某溫室采摘機(jī)器人的運(yùn)動(dòng)控制系統(tǒng)總體方案 191381.1采摘作業(yè)運(yùn)動(dòng)軌跡分析 1223821.2控制系統(tǒng)整體設(shè)計(jì) 5107251.3系統(tǒng)元器件選型 7125251.4系統(tǒng)基本運(yùn)動(dòng)控制 826171.4.1機(jī)器人運(yùn)動(dòng)學(xué)模型 8138491.4.2直行驅(qū)動(dòng)控制 10169901.4.3轉(zhuǎn)向驅(qū)動(dòng)控制 111.1采摘作業(yè)運(yùn)動(dòng)軌跡分析溫室采摘機(jī)器人底盤主要針對(duì)普通番茄作業(yè)，該品種番茄有效株高30~160cm，由于番茄果實(shí)生長高度差異較大，所以可以采用圖2-1所示的升降平臺(tái)結(jié)合機(jī)械臂采摘不同時(shí)期的成熟番茄，這樣通過調(diào)節(jié)升降平臺(tái)即可對(duì)不同高度的番茄進(jìn)行采摘。1.底盤移動(dòng)平臺(tái)1.升降平臺(tái)3.采摘機(jī)械臂圖2-1采摘機(jī)器人整體架構(gòu)經(jīng)查閱，番茄溫室畝產(chǎn)1×104kg，溫室面積按3×40m2計(jì)算，可收番茄1.8×103kg，機(jī)械臂重10~20kg，升降平臺(tái)重30~40kg，底盤重50kg以內(nèi)，則采摘機(jī)器人總體重量約為100kg，根據(jù)機(jī)器人土壤支撐力的限制，暫定最大負(fù)載100kg。計(jì)算可知，番茄成熟后機(jī)器人共需分18次，每次采收100kg成熟番茄來完成整個(gè)日光溫室的采收作業(yè)。圖2-2底盤尺寸示意圖表2-1番茄采摘機(jī)器人底盤基本參數(shù)參數(shù)長寬高底盤質(zhì)量裝載質(zhì)量轉(zhuǎn)向方式驅(qū)動(dòng)方式數(shù)值0.8m0.6m0.45m100kg100kg前輪阿克曼轉(zhuǎn)向后輪轂電機(jī)驅(qū)動(dòng)溫室番茄壟間距寬度為80cm，為使機(jī)器人在操作道中正常行進(jìn)，避免碰觸番茄壟，左右兩側(cè)需各預(yù)留出10cm的空間以滿足底盤行走需要，為此選定底盤尺寸為60cm×80cm，結(jié)構(gòu)與尺寸示意圖如圖2-2所示，具體參數(shù)結(jié)果如表2-1所示。為進(jìn)一步獲取機(jī)器人在溫室作業(yè)時(shí)具體的行進(jìn)路線和啟停間隔等數(shù)據(jù)，需對(duì)番茄種植規(guī)律深入分析。機(jī)器人于壟間作業(yè)圖如圖2-3所示，當(dāng)機(jī)器人停靠在設(shè)定作業(yè)點(diǎn)上時(shí)，所能采摘的最遠(yuǎn)果實(shí)為A、B、C、D。機(jī)械臂末端首先完成A區(qū)域果實(shí)采集，當(dāng)機(jī)械臂末端運(yùn)動(dòng)到果實(shí)B附近時(shí)，機(jī)器人右側(cè)的成熟果實(shí)采摘作業(yè)完畢，左側(cè)同理。1.底盤1.深度相機(jī)3.激光雷達(dá)4.機(jī)械臂5.末端執(zhí)行器6.番茄識(shí)別模塊7.機(jī)械臂運(yùn)動(dòng)控制器8.升降平臺(tái)9.果實(shí)圖2-3機(jī)器人壟間作業(yè)圖附近區(qū)域成熟果實(shí)全部摘取完畢后，機(jī)器人開始規(guī)劃到下一目標(biāo)作業(yè)區(qū)域的最優(yōu)路徑，當(dāng)前壟間過道作業(yè)完成后，機(jī)器人按預(yù)定軌跡進(jìn)入下一個(gè)壟間過道繼續(xù)作業(yè)。1.作業(yè)空間1.末端執(zhí)行器3.機(jī)械臂4.底盤圖2-4溫室番茄自動(dòng)化采摘作業(yè)區(qū)域假設(shè)采摘機(jī)械臂各個(gè)關(guān)機(jī)轉(zhuǎn)動(dòng)到180°位置，轉(zhuǎn)動(dòng)基關(guān)節(jié)，可知機(jī)器人的理論作業(yè)區(qū)域?yàn)槿鐖D3-2所示的圓冠體形狀。為便于連續(xù)規(guī)劃機(jī)器人的采摘作業(yè)軌跡，計(jì)算采摘作業(yè)點(diǎn)，取機(jī)器人實(shí)際的采摘作業(yè)區(qū)域?yàn)橐粋€(gè)長方體，如圖2-4所示。如圖2-5所示，x軸方向上的坐標(biāo)起始點(diǎn)與采摘作業(yè)空間長度的一半20cm相加，可獲得機(jī)器人的下一個(gè)?？奎c(diǎn)的x軸坐標(biāo)；x軸方向上的坐標(biāo)起始點(diǎn)與40cm相加，可獲得機(jī)器人的下一個(gè)采摘作業(yè)終點(diǎn)的x軸坐標(biāo)；最終分析可得，考慮到機(jī)械臂作業(yè)空間以及番茄植株分布，機(jī)器人第一次啟停間隔為20cm，后續(xù)40cm依次遞增，方能完成整個(gè)溫室種植行的番茄采摘。圖2-5番茄采摘作業(yè)區(qū)域與下一個(gè)?？奎c(diǎn)確認(rèn)溫室寬度為4m，且番茄壟端距溫室保溫膜會(huì)留出至少80cm的通道，計(jì)算可得壟長為240cm，根據(jù)作業(yè)間隔40cm，可算得單壟雙列種植行左右兩側(cè)需要各設(shè)置6個(gè)作業(yè)點(diǎn)，即機(jī)器人在操作道中間作業(yè)時(shí)每行需停6次。具體作業(yè)行進(jìn)軌跡圖如圖2-6所示。1-機(jī)器人作業(yè)?？奎c(diǎn)2-機(jī)器人巡航路徑3-番茄種植壟4-壟間過道圖2-6采摘機(jī)器人行進(jìn)路徑示意圖當(dāng)機(jī)器人沿壟間作業(yè)時(shí)，機(jī)械臂作業(yè)精度與停車位置的誤差大小密切相關(guān)，停車位置誤差取決于機(jī)器人的導(dǎo)航定位精度，這也是底盤控制的關(guān)鍵問題，為使機(jī)器人能夠完成溫室番茄采收作業(yè)，提出四項(xiàng)要求。（1）直線行走：車體距壟左右各預(yù)留10cm間隔，以目前激光雷達(dá)導(dǎo)航定位普遍最大誤差6.1cm作為參考（Subramanianetal2006），底盤行進(jìn)誤差需在該范圍內(nèi)；（2）定點(diǎn)停車：根據(jù)作業(yè)空間確定機(jī)器人停車間隔為40cm，后面依次?？?；（3）壟間轉(zhuǎn)向：按壟寬75cm計(jì)算，車體轉(zhuǎn)彎半徑需在67.5~87.5cm范圍之內(nèi)；（4）S型巡航：基于已構(gòu)建地圖設(shè)定全局采摘作業(yè)點(diǎn)，運(yùn)行導(dǎo)航節(jié)點(diǎn)后機(jī)器人需依次抵達(dá)并定點(diǎn)停車。最終根據(jù)采摘機(jī)器人底盤的設(shè)計(jì)參數(shù)，以及四項(xiàng)采摘作業(yè)要求，搭建圖2-7所示的采摘機(jī)器人底盤。圖2-7采摘機(jī)器人底盤1.2控制系統(tǒng)整體設(shè)計(jì)上一節(jié)對(duì)番茄種植環(huán)境、自主采摘機(jī)器人作業(yè)需求進(jìn)行了分析，本系統(tǒng)僅針對(duì)過道較為平坦的溫室環(huán)境下自主作業(yè)機(jī)器人定位導(dǎo)航控制進(jìn)行設(shè)計(jì)。系統(tǒng)架構(gòu)如圖2-8所示，該控制系統(tǒng)主要包括上位機(jī)、下位機(jī)、運(yùn)動(dòng)執(zhí)行、信號(hào)與數(shù)據(jù)采集和監(jiān)控五個(gè)部分。圖2-8控制系統(tǒng)架構(gòu)圖（1）上位機(jī)采用HT780-i5工控機(jī)，安裝Ubuntu18.04與ROS系統(tǒng)，主要負(fù)責(zé)獲取環(huán)境信息以及創(chuàng)建環(huán)境地圖、實(shí)現(xiàn)機(jī)器人的自主定位導(dǎo)航等。（2）下位機(jī)主要負(fù)責(zé)移動(dòng)平臺(tái)的基本運(yùn)動(dòng)控制，具體電路基于STM32芯片進(jìn)行設(shè)計(jì)。（3）運(yùn)動(dòng)執(zhí)行方式為前輪轉(zhuǎn)向后輪驅(qū)動(dòng)，通過四桿機(jī)構(gòu)與舵機(jī)實(shí)現(xiàn)轉(zhuǎn)向，通過獨(dú)立懸掛與輪轂電機(jī)實(shí)現(xiàn)驅(qū)動(dòng)。（4）信號(hào)采集部分獲取雷達(dá)與相機(jī)信息，構(gòu)建環(huán)境地圖，確定所處位置。（5）監(jiān)控端為具有Wi-Fi接收功能的PC，工控機(jī)設(shè)置有開機(jī)自啟Wi-Fi功能，由PC連入工控機(jī)局域網(wǎng)可實(shí)現(xiàn)遠(yuǎn)程監(jiān)控。圖2-9控制系統(tǒng)具體架構(gòu)控制系統(tǒng)詳細(xì)架構(gòu)如圖2-9所示，定位導(dǎo)航與番茄采收流程如圖2-10所示，內(nèi)容主要包括建圖與作業(yè)兩部分。建圖時(shí)PC發(fā)出構(gòu)圖命令，移動(dòng)機(jī)器人根據(jù)遠(yuǎn)程指令在未知環(huán)境中移動(dòng)。上位機(jī)通過融合里程計(jì)、激光與深度測量數(shù)據(jù)建立工作環(huán)境下的地圖。圖2-10采摘機(jī)器人底盤整體控制流程圖作業(yè)時(shí)用戶根據(jù)需要，通過遠(yuǎn)程監(jiān)控界面設(shè)置導(dǎo)航的目標(biāo)作業(yè)點(diǎn)。遠(yuǎn)程啟動(dòng)機(jī)器人的導(dǎo)航進(jìn)程，導(dǎo)入已構(gòu)建的環(huán)境地圖，機(jī)器人根據(jù)傳感器實(shí)時(shí)返回的數(shù)據(jù)開始路徑規(guī)劃及自動(dòng)巡航作業(yè)，到達(dá)目標(biāo)作業(yè)點(diǎn)后，升降平臺(tái)與機(jī)械臂依次啟動(dòng)，完成該點(diǎn)番茄采收，上位機(jī)規(guī)劃行至下一點(diǎn)的路徑，繼續(xù)重復(fù)如上動(dòng)作直至采收完畢。1.3系統(tǒng)元器件選型主控制器作為該番茄采摘機(jī)器人底盤控制系統(tǒng)的上位機(jī)，需要完成地圖構(gòu)建，導(dǎo)航等復(fù)雜任務(wù)，故選用HT780-i7工控機(jī)作為系統(tǒng)控制核心，工控機(jī)性能參數(shù)見表2-2。表2-2工控機(jī)性能參數(shù)參數(shù)CPU主頻顯存內(nèi)存電源功率主板結(jié)構(gòu)數(shù)值i7-65001.50GHzGDDR3/2GBDDR3/8GB60WMini-ITX考慮到溫室環(huán)境中的道路尺寸、植物形狀等因素，激光雷達(dá)的掃描范圍應(yīng)在5m以上，測距誤差不超過1mm，角度誤差不超過1°，本系統(tǒng)選用上海思嵐公司生產(chǎn)的RPLIDARA2二維激光雷達(dá)，激光雷達(dá)參數(shù)見表2-3。表2-3A2激光雷達(dá)參數(shù)表參數(shù)掃描角度測距分辨率角度分辨率單次測距時(shí)間測量頻率掃描頻率測距范圍數(shù)值0-360°0.5mm0.9°0.25ms>4000HZ10HZ0.15~8m考慮到番茄種植環(huán)境的復(fù)雜性，深度相機(jī)選取精度較高、體積較小以及成本較低的AstraPro相機(jī)，具有紅外感知模組、物塊接近感知模組、RGB相機(jī)模組以及IR相機(jī)模組，相對(duì)于其他深度相機(jī)具有較寬闊的深度范圍和視場角，生成的深度圖質(zhì)量比較高，具體參數(shù)見表2-4所示。表2-4AstraPro參數(shù)表參數(shù)深度范圍深度分辨率深度視場角圖像分辨率傳輸接口相機(jī)尺寸數(shù)值0.6-8m1280×102458.4×45.5cm1920×1080USB1.0160×40×30mm機(jī)器人收獲的果實(shí)數(shù)量較多，必須擁有良好的負(fù)重能力，后輪集中式驅(qū)動(dòng)更具優(yōu)勢，驅(qū)動(dòng)與轉(zhuǎn)向完全分隔開，電機(jī)只需提供縱向動(dòng)力，舵機(jī)只需提供橫向動(dòng)力，本文選取中菱科技生產(chǎn)的ZLAC8015D的輪轂電機(jī)套裝，具體參數(shù)如表2-5所示：表2-5電機(jī)的參數(shù)額定電壓額定轉(zhuǎn)矩瞬間最大轉(zhuǎn)矩額定轉(zhuǎn)速額定最大轉(zhuǎn)速建議負(fù)載24-48VDC22N·m44N·m200RPM300RPM200kg1.車輪1.轉(zhuǎn)向連桿3.轉(zhuǎn)向軸4.舵機(jī)5.固定軸圖2-11舵機(jī)轉(zhuǎn)向機(jī)構(gòu)俯視圖為保證采摘機(jī)器人具有較高的采收效率與最大200kg的載重，采用連桿式轉(zhuǎn)向機(jī)構(gòu)與大扭矩舵機(jī)作為轉(zhuǎn)向驅(qū)動(dòng)力，舵機(jī)轉(zhuǎn)向機(jī)構(gòu)俯視圖如圖2-11所示。為保證安全行駛選，選擇60kg單軸舵機(jī)。故本文采用型號(hào)為DS5160全金屬齒輪舵機(jī)，性能參數(shù)如表2-6所示。表2-6舵機(jī)性能參數(shù)參數(shù)型號(hào)工作電壓工作頻率最大轉(zhuǎn)角扭力數(shù)值DS5160DC4.8~60V1520/330Hz270°60kg/cm1.4系統(tǒng)基本運(yùn)動(dòng)控制本設(shè)計(jì)采用前輪舵機(jī)轉(zhuǎn)向，后輪輪轂驅(qū)動(dòng)方式，上位機(jī)通過CAN向下位機(jī)發(fā)送底盤下一時(shí)刻需抵達(dá)的(x,y,z)坐標(biāo)，機(jī)器人根據(jù)里程計(jì)模型反解出所需要的前輪轉(zhuǎn)向角與后輪驅(qū)動(dòng)速度，以此對(duì)舵機(jī)與輪轂電機(jī)調(diào)控。1.4.1機(jī)器人運(yùn)動(dòng)學(xué)模型由于采摘機(jī)器人底盤運(yùn)動(dòng)限制在世界坐標(biāo)系中，忽略Z軸方向的運(yùn)動(dòng)，建立世界坐標(biāo)系與機(jī)器人坐標(biāo)系（朱亞坤2018）。圖2-12機(jī)器人底盤運(yùn)動(dòng)模型底盤運(yùn)動(dòng)模型如圖2-12所示。為滑移角，指底盤速度方向和朝向兩者間所成的角度，為航向角，指底盤與軸的夾角。為后軸中心坐標(biāo)，為航向角，為底盤的角速度，為車速，為前輪轉(zhuǎn)角，為橫擺角速度，當(dāng)?shù)妆P為前輪驅(qū)動(dòng)時(shí)，可假設(shè)后輪轉(zhuǎn)角恒為0。由正弦法則結(jié)合運(yùn)動(dòng)學(xué)模型可列出以下式子：(2-1)(2-2)展開公式可得：(2-3)(2-4)聯(lián)立公式2-3，2-4可得：(2-5)因?yàn)榈妆P的方向變化率等于底盤的角速度，則底盤的角速度為:(2-6)聯(lián)立公式2-5、2-6可得：(2-7)則在世界坐標(biāo)系下，可得底盤運(yùn)動(dòng)學(xué)模型，此模型中有三個(gè)輸入量：前輪偏角、后輪偏角和速度。(2-8)滑移角可由公式求得：(2-9)將后輪中心點(diǎn)設(shè)置為參考點(diǎn)，上述滑移角極小，假設(shè)為0。以此對(duì)底盤運(yùn)動(dòng)模型進(jìn)行簡化。狀態(tài)量為，被控量為時(shí)，簡化底盤運(yùn)動(dòng)學(xué)模型如式所示：(2-10)底盤控制對(duì)象一般表示為，運(yùn)動(dòng)學(xué)模型可簡化為：(2-11)可知速度的控制主要通過加速、減速及剎車等來控制，橫擺角速度主要通過底盤轉(zhuǎn)角來控制。1.4.2直行驅(qū)動(dòng)控制后輪輪轂電機(jī)控制模式采用的PWM脈沖位置控制模式，通過STM32定時(shí)器產(chǎn)生不同頻率的PWM脈沖，發(fā)送給電機(jī)驅(qū)動(dòng)器。我們根據(jù)所求得的后輪驅(qū)動(dòng)速度計(jì)算所需的脈沖頻率，實(shí)現(xiàn)輪轂電機(jī)不同速度的調(diào)控。圖2-13PWM頻率控制模式示意圖以本文應(yīng)用的STM32F103RCT6控制芯片以及ZLAC8015D輪轂電機(jī)為例，可知輪轂電機(jī)轉(zhuǎn)一圈需5600個(gè)脈沖，為將實(shí)際轉(zhuǎn)速與脈沖頻率結(jié)合，設(shè)輪轂半徑為，則前進(jìn)1m共需個(gè)脈沖，當(dāng)需要機(jī)器人以速度前進(jìn)時(shí)，可得此時(shí)需要輸入驅(qū)動(dòng)器的脈沖頻率為：(2-12)本文通過RCT6芯片TIM1時(shí)鐘產(chǎn)生所需要頻率的脈沖，分頻系數(shù)取值范圍為1-65535，通過TIM2時(shí)鐘記錄TIM1產(chǎn)生脈沖個(gè)數(shù)，當(dāng)達(dá)到設(shè)定的脈沖個(gè)數(shù)后關(guān)閉時(shí)鐘，RCT6芯片時(shí)鐘脈沖頻率計(jì)算公式為：(2-13)為芯片時(shí)鐘參數(shù)，為方便計(jì)算令，為程序中用于調(diào)節(jié)TIM1頻率的輸入?yún)⒘?，結(jié)合公式2-12可得：(2-14)1.4.3轉(zhuǎn)向驅(qū)動(dòng)控制要保證底盤轉(zhuǎn)向系統(tǒng)能夠根據(jù)所設(shè)定的軌跡合理轉(zhuǎn)向，需要考慮車體長寬、航速、轉(zhuǎn)彎半徑等信息，共同對(duì)舵機(jī)輸出角度進(jìn)行控制。簡化底盤幾何模型如圖2-14所示。前輪轉(zhuǎn)向角與后軸遵循曲率之間的幾何關(guān)系式為：圖2-14簡化底盤幾何模型(2-15)為轉(zhuǎn)彎半徑，表示前輪的轉(zhuǎn)角，為軸距。從簡化的幾何模型出發(fā)，過后軸中心點(diǎn)沿路徑做切線并相較于切點(diǎn)，通過控制前輪轉(zhuǎn)角，使底盤沿著規(guī)劃好的路徑行駛，如圖2-15所示。圖中是下一個(gè)要到達(dá)的作業(yè)點(diǎn)，該點(diǎn)位于已經(jīng)規(guī)劃好的全局路徑上，表示車輛當(dāng)前位置到目標(biāo)點(diǎn)的距離，表示車身姿態(tài)和目標(biāo)點(diǎn)的夾角，根據(jù)正弦定理可得：圖2-15底盤與路徑關(guān)系(2-16)化簡得：(2-17)因?yàn)榈缆非蕿椋鲜揭部杀硎緸椋?2-18)可得：(2-19)加入時(shí)間變量可得：(2-20)在知道時(shí)刻車身和目標(biāo)點(diǎn)的夾角和距離目標(biāo)點(diǎn)的前視距離的情況下，可以依據(jù)上式計(jì)算前輪轉(zhuǎn)角。為更好的控制舵機(jī)轉(zhuǎn)角，使車底盤穩(wěn)定，定義一個(gè)新的量，即車輛當(dāng)前姿態(tài)和目標(biāo)點(diǎn)在橫向上的誤差，由此可得夾角正弦為：(2-21)則曲率可表示為：(2-22)由上式可知該舵機(jī)轉(zhuǎn)角控制其實(shí)是一個(gè)橫向的控制器，系數(shù)為，該控制器受到前視距離的影響很大，而是速度函數(shù)，不同車速需對(duì)應(yīng)不同的前視距離。為此將前視距離表示成車輛縱向速度的線形函數(shù)，即，得到當(dāng)前時(shí)間下舵機(jī)需要輸出的轉(zhuǎn)向角公式：(2-23)最終得到舵機(jī)轉(zhuǎn)角計(jì)算公式，選取0.3~1.0m/s不同的轉(zhuǎn)向速度，計(jì)算對(duì)應(yīng)速度下的前輪轉(zhuǎn)向輸出角、向心加速度及最大側(cè)向力。由于公式中有較多未知量，需進(jìn)一步取代與化簡，聯(lián)立式2-19和2-21可得(2-24)查閱文獻(xiàn)可知，設(shè)定預(yù)瞄距離選取方式可以表現(xiàn)為如式2-25：(2-25)上式中常數(shù)，為最大制動(dòng)加速度，表示最短制動(dòng)距離。表示反應(yīng)時(shí)間，則為車輛反應(yīng)距離，表示車輛的最小轉(zhuǎn)彎半徑，聯(lián)立式2-24和2-25并帶入時(shí)間量可得：(2-26)由于底盤轉(zhuǎn)向行駛規(guī)定為勻速定曲率行駛，故底盤車體縱向速度為我們所設(shè)