本發(fā)明屬于微型傳感器應(yīng)用和智能機器人控制領(lǐng)域，具體涉及一種用于草坪維護(hù)的智能割草機的直線行走控制系統(tǒng)。
背景技術(shù)：
：城市綠化是城市化建設(shè)過程中必不可少的一環(huán)，而草坪具有吸塵、降噪、保濕、保持水土和凈化環(huán)境等諸多優(yōu)點，在城市綠化中占據(jù)著舉足輕重的地位。隨著城市草坪面積的增大，草坪的日常維護(hù)和保養(yǎng)成了擺在人們尤其是園藝工作者面前的一項主要工作，這種工作勞動強度大、重復(fù)性高，使用傳統(tǒng)的機械式割草機進(jìn)行草坪維護(hù)不僅消耗大量的人力、物力，而且還會產(chǎn)生較大的噪聲，不利于保護(hù)環(huán)境。隨著信息技術(shù)和智能控制技術(shù)的發(fā)展，各種智能割草機不斷涌現(xiàn)，為草坪的日常維護(hù)和保養(yǎng)帶來了巨大的方便，不僅節(jié)省了大量的勞動力，而且能消除機械式割草機的噪聲。通常智能割草機在進(jìn)行草坪維護(hù)時需要執(zhí)行復(fù)雜的動態(tài)路徑規(guī)劃策略，以盡可能地完全覆蓋草坪區(qū)域，但也不可避免地導(dǎo)致了草坪區(qū)域重復(fù)覆蓋的問題，從而降低了作業(yè)效率。如果能夠?qū)崿F(xiàn)智能割草機的直線行走，并執(zhí)行平行往復(fù)式的割草運動，如圖1所示，則可以避免執(zhí)行復(fù)雜的動態(tài)路徑規(guī)劃策略，降低重復(fù)覆蓋的草坪區(qū)域，從而大大提高作業(yè)效率。然而現(xiàn)有的可能實現(xiàn)割草機直線行走的控制策略，如基于通電導(dǎo)線的電磁感應(yīng)、光電編碼器和視覺導(dǎo)航等，在室外草坪上需要布設(shè)通電導(dǎo)線，否則無法有效識別割草機的偏轉(zhuǎn)或缺乏相應(yīng)的視覺參考，因而對割草機直線行走的控制效果均不太理想。另外，基于微機電技術(shù)MEMS的慣性/磁傳感器MARG通常包括三軸加速度傳感器、三軸角速率傳感器即陀螺儀和三軸磁傳感器，基于這三種傳感器進(jìn)行的姿態(tài)檢測具有低成本、低功耗、尺寸小和靈敏度高的特點，而且在進(jìn)行姿態(tài)檢測時不需要外部設(shè)備的輔助，因而在無人機、智能機器人等諸多領(lǐng)域都得到了廣泛的應(yīng)用。基于現(xiàn)有技術(shù)的現(xiàn)狀，本申請的發(fā)明人擬提供一種基于慣性/磁傳感器MARG姿態(tài)檢測的智能割草機直線行走控制系統(tǒng)，將基于MARG的姿態(tài)檢測應(yīng)用于智能割草機的控制中，不僅可以有效控制割草機的直線行走，而且能實時檢測 割草機的姿態(tài)，防止割草機發(fā)生側(cè)翻，從而為割草機進(jìn)行草坪維護(hù)提供很大的幫助。技術(shù)實現(xiàn)要素：本發(fā)明的目的在于針對現(xiàn)有技術(shù)中由于割草機驅(qū)動電機和驅(qū)動輪參數(shù)的差異、路面坡度以及電源電量等導(dǎo)致的割草機無法走直線的問題，以及基于通電導(dǎo)線的電磁感應(yīng)、光電編碼器和視覺導(dǎo)航等在室外草坪上對割草機直線行走控制效果不理想的問題，提供一種基于慣性/磁傳感器MARG姿態(tài)檢測的割草機直線行走控制系統(tǒng)。該系統(tǒng)的主要執(zhí)行機構(gòu)包括基于慣性/磁傳感器MARG的姿態(tài)檢測模塊、割草機主控單元和電機驅(qū)動模塊，需要執(zhí)行的兩大主要程序包塊割草機姿態(tài)檢測算法和航向偏轉(zhuǎn)調(diào)整系統(tǒng)。其中，割草機姿態(tài)檢測主要通過MARG姿態(tài)檢測模塊完成，該模塊中的處理器首先采集三軸加速度傳感器、陀螺儀和三軸磁傳感器的測量數(shù)據(jù)并進(jìn)行預(yù)處理，然后執(zhí)行姿態(tài)解算算法，獲得割草機的實時姿態(tài)信息，主要包括滾轉(zhuǎn)角roll、俯仰角pitch和偏航角yaw，最后將割草機的三個姿態(tài)角通過串口發(fā)送給割草機的主控單元，供主控單元實現(xiàn)割草機的直線行走控制。另外，割草機的航向偏轉(zhuǎn)調(diào)整主要通過割草機的主控單元和電機驅(qū)動模塊實現(xiàn)。主控單元從MARG姿態(tài)檢測模塊接收割草機的實時姿態(tài)角，以姿態(tài)角中的偏航角yaw控制割草機的航向，在航向發(fā)生偏轉(zhuǎn)時進(jìn)行航向調(diào)整和由于航向偏轉(zhuǎn)導(dǎo)致的航線偏移距離的補償，以滾轉(zhuǎn)角roll判斷割草機的負(fù)重驅(qū)動輪，當(dāng)割草機在斜坡上進(jìn)行航向調(diào)整時，通過相應(yīng)的比例因子補償負(fù)重輪的電機調(diào)整轉(zhuǎn)速，從而使割草機更好地適應(yīng)不同路面狀況下的直線行走；最終，主控單元產(chǎn)生的所有電機控制策略均由電機驅(qū)動模塊驅(qū)動電機實現(xiàn)。本發(fā)明的主要優(yōu)點有：通過慣性/磁傳感器MARG實現(xiàn)割草機的姿態(tài)檢測，不僅具有低成本、低功耗、尺寸小和靈敏度高的特點，而且還不需要外部設(shè)備的輔助，為割草機的大范圍自由移動提供了很大的便利；可以實時檢測割草機的姿態(tài)，從而防止割草機在大傾角草坪上作業(yè)時發(fā)生側(cè)翻或?qū)Πl(fā)生側(cè)翻的情況進(jìn)行報警；由于通過姿態(tài)檢測算法獲得的姿態(tài)是相對于地球坐標(biāo)系的姿態(tài)，因而其偏航角不受地面狀況的影響，可以實時準(zhǔn)確地確定割草機的航向，從而有效實現(xiàn)直線行走控制。附圖說明圖1為割草機做平行往復(fù)式運動的示意圖。圖2為割草機控制系統(tǒng)結(jié)構(gòu)示意圖。圖3為基于MARG的改進(jìn)高斯牛頓互補濾波姿態(tài)檢測算法的計算流程示意圖。圖4為MARG姿態(tài)檢測模塊與車身相對位置的示意圖。圖5為割草機航向偏移調(diào)整示意圖。圖6為割草機偏移距離和偏轉(zhuǎn)角度關(guān)系的示意圖。圖7為割草機航向調(diào)整控制程序的流程圖。具體實施方式本發(fā)明所需要的執(zhí)行機構(gòu)的總體結(jié)構(gòu)框圖如圖2所示，下面結(jié)合圖2介紹基于慣性/磁傳感器MARG姿態(tài)檢測的割草機直線行走控制系統(tǒng)的具體實現(xiàn)方式。實施例1(1)基于慣性/磁傳感器MARG的割草機姿態(tài)檢測該部分主要通過圖2中的MARG姿態(tài)檢測模塊實現(xiàn)。為了通過MARG姿態(tài)模塊檢測割草機的姿態(tài)信息，需要將MARG姿態(tài)檢測模塊放置于割草機平臺上，并使其中的某個軸如X軸與割草機的中軸線平行，Y軸沿著橫向向右，Z軸垂直向上，類似如圖3的形式，以準(zhǔn)確地獲得割草機隨著地面狀況的變化而導(dǎo)致的姿態(tài)變化；由MARG姿態(tài)檢測模塊中的處理器如STM32F10X系列處理器以50Hz的采樣頻率采集三個傳感器的測量數(shù)據(jù)，并進(jìn)行預(yù)處理，主要包括低通濾波和傳感器測量數(shù)據(jù)校準(zhǔn)，以消除高頻噪聲干擾和尖峰毛刺信號以及補償傳感器的系統(tǒng)誤差。然后執(zhí)行基于MARG傳感數(shù)據(jù)的姿態(tài)解算算法，如基于改進(jìn)高斯牛頓法的姿態(tài)檢測算法和基于Kalman濾波的姿態(tài)檢測算法等，獲得割草機的實時姿態(tài)信息，主要包括割草機的滾轉(zhuǎn)角roll、俯仰角pitch和偏航角yaw，最后將得到的姿態(tài)角通過通用串口傳輸給割草機的主控單元；對于上述基于MARG傳感數(shù)據(jù)的姿態(tài)檢測算法，采用改進(jìn)的高斯牛頓互補濾波姿態(tài)檢測算法，該算法精度高且計算量較小，能夠有效地應(yīng)用于處理能力有限、對實時性要求高的可移動式設(shè)備中，所述算法首先通過三軸加速度和三軸磁傳感器校準(zhǔn)后的歸一化測量值，通過改進(jìn)的高斯牛頓算法--主要是降低算法的迭代次數(shù)和矩陣運算的維度，獲得類似梯度下降算法的梯度因子然后乘以一個表示陀螺儀零均值測量誤差的比例因子β，對由陀螺儀測量數(shù)據(jù)獲得的姿態(tài)變化率進(jìn)行修正，最后將修正后的估計姿態(tài)變化率進(jìn)行一階時間積分運算并歸一化，獲得由四元數(shù)表示的割草機當(dāng)前時刻的姿態(tài)再將得到的四元數(shù) 表示的姿態(tài)轉(zhuǎn)換為歐拉角表示的姿態(tài)，最終用于割草機的直線行走控制中；在上述由高斯牛頓算法獲得梯度因子的同時，使用該梯度因子通過算式并進(jìn)行一階時間積分運算再乘以一個表示陀螺儀非零均值測量誤差變化率的比例因子ζ，用于修正陀螺儀的測量數(shù)據(jù)誤差，以進(jìn)一步降低陀螺儀的漂移誤差對最終姿態(tài)檢測精度的影響；根據(jù)上述內(nèi)容，本發(fā)明中所采用的基于慣性/磁傳感器MARG的姿態(tài)檢測算法的總體計算過程可以概括為如圖3所示的流程示意圖；(2)割草機航向偏轉(zhuǎn)調(diào)整控制該部分主要通過圖2中的主控單元和電機驅(qū)動模塊實現(xiàn)。主控單元通過串口從MARG姿態(tài)檢測模塊接收姿態(tài)角roll、pitch和yaw數(shù)據(jù)，在割草機即將進(jìn)行的某段直線行走開始處，記錄偏航角yaw前N1個數(shù)據(jù)的平均值作為割草機進(jìn)行該段直線行走的初始航向角yaw_init，然后在割草機行進(jìn)過程中實時計算當(dāng)前航向角與初始航向角差值的絕對值以及當(dāng)前N2個滾轉(zhuǎn)角的平均值roll_avg，以差值絕對值判斷割草機是否發(fā)生航向偏轉(zhuǎn)，滾轉(zhuǎn)角平均值確定割草機的負(fù)重輪；當(dāng)割草機當(dāng)前航向角與初始航向角差值的絕對值沒有超過預(yù)先設(shè)定的閾值角θth或沒有連續(xù)超過預(yù)先設(shè)定的閾值角θthN3次時，認(rèn)為割草機航向沒有發(fā)生偏轉(zhuǎn)，此時割草機航向調(diào)整標(biāo)志Adjust_Direction＝0，割草機繼續(xù)直線前進(jìn)，不進(jìn)行航向調(diào)整；當(dāng)割草機當(dāng)前航向角與初始航向角差值的絕對值連續(xù)超過預(yù)先設(shè)定的閾值角θthN3次時，認(rèn)為割草機航向發(fā)生了偏轉(zhuǎn)，需要向相反的方向調(diào)整航向。此時根據(jù)如圖4所示的MARG姿態(tài)檢測模塊與割草機平臺相對擺放位置的對應(yīng)關(guān)系以及割草機當(dāng)前航向角yaw與初始航向角yaw_init的差值yaw-yaw_init的正負(fù)分別進(jìn)行如下兩種狀況的航向調(diào)整：(i)若yaw-yaw_init>0，則設(shè)置航向調(diào)整標(biāo)志Adjust_Direction＝1，表示割草機航向向左偏，需要向右調(diào)整航向。此時再查看前述計算出的滾轉(zhuǎn)角均值roll_avg，若roll_avg＝0，則認(rèn)為割草機兩個驅(qū)動輪的負(fù)重相同，割草機在水平面上行進(jìn)，此時左驅(qū)動輪電機以pl+p0的轉(zhuǎn)速進(jìn)行航向調(diào)整(這里的pl表示割草機在水平面上直線前進(jìn)時，左驅(qū)動輪的電機轉(zhuǎn)速)，直至調(diào)整至越過初始航向角某個角度α；若||roll_avg||>0，表示割草機在斜坡上行進(jìn)，此時負(fù)重輪的負(fù)重相比非負(fù)重輪大，導(dǎo)致由負(fù)重輪向上坡方向調(diào)整航向時，所需要的功率相比在水平面上調(diào)整航向所需的功率較大，因而采用比例因子ki對割草機在斜坡上進(jìn)行航向調(diào)整時的電機轉(zhuǎn)速進(jìn)行補償。在割草機航向向左偏的情況下，ki具有兩個值kll、krl，分別對應(yīng)左輪負(fù)重左偏、右輪負(fù)重左偏的情況，再根據(jù)roll_avg的正負(fù)以及割草機航向偏轉(zhuǎn)的對應(yīng)關(guān)系，可以確定：當(dāng)roll_avg>0時，對應(yīng)左輪負(fù)重左偏的情況，此時左驅(qū)動輪電機以pl+p0+kll*||roll_avg||的轉(zhuǎn)速進(jìn)行航向調(diào)整，直 至調(diào)整至越過初始航向角某個角度α；當(dāng)roll_avg<0時，對應(yīng)右輪負(fù)重左偏的情況，此時左驅(qū)動輪電機以pl+p0+krl*||roll_avg||的轉(zhuǎn)速進(jìn)行航向調(diào)整，直至調(diào)整至越過初始航向角某個角度α；(ii)若yaw-yaw_init<0，則設(shè)置航向調(diào)整標(biāo)志Adjust_Direction＝2，表示割草機航向向右偏，需要向左調(diào)整航向。此時同樣查看前述計算出的滾轉(zhuǎn)角均值roll_avg，若roll_avg＝0，則認(rèn)為割草機兩個驅(qū)動輪的負(fù)重相同，割草機在水平面上行進(jìn)，此時右驅(qū)動輪電機以pr+p0的轉(zhuǎn)速進(jìn)行航向調(diào)整(這里的pr表示割草機在水平面上直線前進(jìn)時，右驅(qū)動輪的電機轉(zhuǎn)速)，直至調(diào)整至越過初始航向角某個角度α；若||roll_avg||>0，與(i)的狀況類似，也采用比例因子ki對割草機在斜坡上進(jìn)行航向調(diào)整時的電機轉(zhuǎn)速進(jìn)行補償。在這種割草機航向向右偏的情況下，ki也具有兩個值klr、krr，分別對應(yīng)左輪負(fù)重右偏、右輪負(fù)重右偏的情況，再根據(jù)roll_avg的正負(fù)以及割草機航向偏轉(zhuǎn)的對應(yīng)關(guān)系，可以確定：當(dāng)roll_avg>0時，對應(yīng)左輪負(fù)重右偏的情況，此時右驅(qū)動輪電機以pr+p0+klr*||roll_avg||的轉(zhuǎn)速進(jìn)行航向調(diào)整，直至調(diào)整至越過初始航向角某個角度α；當(dāng)roll_avg<0時，對應(yīng)右輪負(fù)重右偏的情況，此時右驅(qū)動輪電機以pr+p0+krr*||roll_avg||的轉(zhuǎn)速進(jìn)行航向調(diào)整，直至調(diào)整至越過初始航向角某個角度α；在割草機進(jìn)行航向調(diào)整時，采用如圖5所示的邊行進(jìn)邊調(diào)整航向的策略，即圖中的“①→②→③”，在航向調(diào)整的過程中，如果僅僅將割草機航向調(diào)整到初始航向就停止調(diào)整，由于此時割草機已經(jīng)偏離了原來的航線，則會產(chǎn)生航線漂移，如圖5所示，即在開始調(diào)整割草機航向的位置“①”處，割草機已經(jīng)偏離了原來航線d的距離，因而為了避免這種情況的產(chǎn)生，必須將割草機的航向調(diào)整到越過初始航向某個角度α，正如上述(i)和(ii)狀況中所述，然后再繼續(xù)前進(jìn)一段時間，以補償割草機航向偏轉(zhuǎn)導(dǎo)致的偏移距離；在割草機驅(qū)動輪半徑r和驅(qū)動電機轉(zhuǎn)速ω一定的情況下，割草機的偏移距離d和偏移時間Δt具有固定的關(guān)系，如下(1)式。d=R(1-cosθth)=1-cosθthθthlR=1-cosθthθth·r·ω·Δt]]>其中，lR表示割草機偏離初始航向θth角時右驅(qū)動輪轉(zhuǎn)過的距離；此外，在進(jìn)行航向調(diào)整時，若以與發(fā)生偏離時左右驅(qū)動輪相同的相對轉(zhuǎn)速進(jìn)行調(diào)整，則割草機偏離原來航線的最大距離d'＝2d，如圖6所示，然后再繼續(xù)邊行進(jìn)邊調(diào)整航向至越過初始航向角度α，并繼續(xù)行進(jìn)，以補償總體的偏移距離d'，使割草機回到原來的航線；在理想情況下，當(dāng)調(diào)整航向至越過初始航向角 度α?xí)r，再繼續(xù)行進(jìn)2Δt的時間，割草機即可回到原來航線，但由于驅(qū)動輪半徑差異、電機轉(zhuǎn)速差異和路面坡度等各種因素的影響，割草機并不能回到原來的航線或越過了原來的航線，因而用于補償偏移距離的實際前進(jìn)時間需要通過具體的實驗進(jìn)行稍作調(diào)整確定，而且在補償割草機偏移距離時，可以讓割草機調(diào)整至稍微越過初始航線，從而做近似“S”型的前進(jìn)運動，效果會更好。根據(jù)上述內(nèi)容，本發(fā)明中基于慣性/磁傳感器MARG姿態(tài)檢測的割草機直線行走的總體控制流程可以概括為如圖7所示的流程圖。當(dāng)前第1頁1 2 3