趙辰彥，姚立健，楊自棟，顏 松，蔡存成

（浙江農(nóng)林大學(xué) 工程學(xué)院，浙江 杭州 311300）

近年來，中國(guó)溫室生產(chǎn)裝備發(fā)展迅速，智能農(nóng)機(jī)在提高溫室生產(chǎn)經(jīng)濟(jì)效益方面發(fā)揮越來越重要的作用[1]。自動(dòng)導(dǎo)航技術(shù)正成為智能化溫室裝備的研究熱點(diǎn)，其中位置測(cè)量和導(dǎo)航路徑跟蹤控制是自動(dòng)導(dǎo)航系統(tǒng)的兩大核心技術(shù)[2]。在位置測(cè)量方面，全球定位系統(tǒng)(global position system，GPS)[3]被廣泛用于自動(dòng)導(dǎo)航農(nóng)機(jī)定位研究中，但是大多用于室外大田環(huán)境，信號(hào)在室內(nèi)衰減嚴(yán)重，定位效果不佳[4]，在室內(nèi)應(yīng)用受到了限制。許多學(xué)者選擇機(jī)器視覺[5]、紅外[6]、電磁[7]和無線傳感[8-9]等實(shí)現(xiàn)室內(nèi)定位方法。機(jī)器視覺可以檢測(cè)試驗(yàn)環(huán)境中豐富的信息，但是受到光照等不可控因素影響較大。紅外和電磁需預(yù)先鋪設(shè)引導(dǎo)線，更改或擴(kuò)充路徑麻煩，靈活性較差。超寬帶(ultra-wide band，UWB)是一種室內(nèi)無線定位方法，其數(shù)據(jù)傳輸速度快且精度高，適合于溫室環(huán)境定位使用。林相澤等[10]使用UWB在溫室內(nèi)動(dòng)態(tài)靜態(tài)定位精度為6.6 cm，動(dòng)態(tài)定位大部分距離誤差在15.0 cm以上。孫小文等[11]在倉儲(chǔ)環(huán)境內(nèi)為自動(dòng)導(dǎo)航車(AGV)定位動(dòng)態(tài)試驗(yàn)精度為8.5 cm。在導(dǎo)航控制方面，純追蹤[12]是一種基于幾何運(yùn)算的方法，理論知識(shí)不復(fù)雜，但是前視距離的確定較困難。模糊控制是建立在以人的知識(shí)及經(jīng)驗(yàn)的基礎(chǔ)上的一種智能控制方法，有較強(qiáng)的魯棒性和容錯(cuò)性。石翔等[13]以橫向和航向偏差為輸入，根據(jù)專家經(jīng)驗(yàn)制定模糊規(guī)則輸出前視距離；熊中剛等[14]采用模糊免疫PID控制算法實(shí)現(xiàn)前輪轉(zhuǎn)向?qū)Ш娇刂疲本€跟蹤偏差不大于4.0 cm，曲線跟蹤偏差平均偏差為12 cm；XUE等[15]結(jié)合模糊控制與機(jī)器視覺，設(shè)計(jì)了機(jī)器人的路徑跟蹤控制器；ABIYEV等[16]以誤差和誤差變化率為輸入量，速度為輸出量設(shè)計(jì)模糊控制器，為自行設(shè)計(jì)的機(jī)器人進(jìn)行導(dǎo)航控制，沿著30.0 m的引導(dǎo)線行駛最大引導(dǎo)誤差為15.8 mm。上述研究未充分考慮不同工況時(shí)輸入量之間的權(quán)重，且采用固定模糊控制規(guī)則，無法對(duì)模糊控制規(guī)則進(jìn)行自調(diào)整。本研究采用超寬帶技術(shù)構(gòu)建無線定位系統(tǒng)，獲取溫室生產(chǎn)平臺(tái)的位置信息，采用一種帶有自調(diào)整函數(shù)的模糊控制器自動(dòng)調(diào)整各輸入變量的權(quán)重，以實(shí)現(xiàn)最佳的前輪轉(zhuǎn)向角輸出。最后采用實(shí)車進(jìn)行直線和矩形路徑跟蹤試驗(yàn)，并與傳統(tǒng)純追蹤方法進(jìn)行對(duì)比，以驗(yàn)證本研究方法的有效性。

1 材料與方法

1.1 試驗(yàn)材料

試驗(yàn)平臺(tái)為純電動(dòng)4輪機(jī)構(gòu)，采用前輪轉(zhuǎn)向后輪驅(qū)動(dòng)，由鋰電池(48 V，20 Ah)供電，車體長(zhǎng)寬高為1 300 mm×700 mm×1 250 mm，輪距和軸距分別為550 mm、840 mm，如圖1所示。采用電動(dòng)推桿推動(dòng)梯形4桿機(jī)構(gòu)實(shí)現(xiàn)轉(zhuǎn)向，推桿行程為200 mm，由L298N驅(qū)動(dòng)器驅(qū)動(dòng)轉(zhuǎn)向角范圍為-32°～32°。后輪由直流電機(jī)通過減速器與差速器驅(qū)動(dòng)，驅(qū)動(dòng)器型號(hào)為AQMD6020BLS。平臺(tái)的位置信息分別由超寬帶無線定位系統(tǒng)和電子陀螺儀提供。超寬帶模塊數(shù)據(jù)傳輸頻率為1～50 Hz，陀螺儀(WT901C)為平臺(tái)提供航向信息，測(cè)量范圍為-180°～180°，精度為0.1°。采用STM32F767IGT6單片機(jī)作為試驗(yàn)平臺(tái)的主控制器?？刂葡到y(tǒng)原理如圖2所示。

圖1 生產(chǎn)平臺(tái)Figure 1 Prodsuction platform

1.2 定位方法

1.2.1 定位原理 超寬帶無線定位系統(tǒng)是由若干固定基站BS和移動(dòng)標(biāo)簽MS通過一定的通信方式組成。根據(jù)超寬帶定位原理，獲取移動(dòng)標(biāo)簽3維定位坐標(biāo)至少需要4個(gè)基站。由于本研究默認(rèn)生產(chǎn)平臺(tái)行駛在水平地面上，因此定位只需用到水平面橫、縱2個(gè)坐標(biāo)參數(shù)。增加基站的數(shù)量可提高定位的精度。本研究采用4個(gè)基站BS1～BS4組成1個(gè)平面定位坐標(biāo)系OXY，如圖3所示。

圖2 控制系統(tǒng)原理圖Figure 2 Block diagram of control system

圖3 試驗(yàn)區(qū)域Figure 3 Test area

1.2.2 信號(hào)到達(dá)時(shí)間差定位算法 UWB定位方法包含：信號(hào)到達(dá)時(shí)間(TOA)[17]、信號(hào)到達(dá)時(shí)間差(TDOA)[18]、信號(hào)到達(dá)角度(AOA)[19]、接收信號(hào)強(qiáng)度(RSSI)[20]等。AOA定位算法主要參考基站天線陣列獲得信號(hào)角度，對(duì)基站位置依賴較大，且受到非視距傳播影響，定位精度不高。RSSI定位算法簡(jiǎn)易，成本不高，但是用作室內(nèi)定位時(shí)，易受多徑傳播和信號(hào)反射的影響，定位精度誤差較大。TOA與TDOA較為常用，時(shí)間分辨率高；TDOA較TOA的優(yōu)勢(shì)在于，不需要基站與移動(dòng)標(biāo)簽時(shí)鐘同步[21]，因此，本研究采用TDOA的定位方法。TDOA定位也稱雙曲線定位，基本思想為：任選2個(gè)坐標(biāo)已知的基站為某雙曲線左右焦點(diǎn)，2基站的距離值即為雙曲線焦距值。通過測(cè)量這2個(gè)基站信號(hào)到達(dá)移動(dòng)標(biāo)簽的時(shí)間差得到移動(dòng)標(biāo)簽到2個(gè)基站的距離差，根據(jù)雙曲線的定義可求得實(shí)半軸值，并獲得該雙曲線方程。用1個(gè)坐標(biāo)已知的新基站替換上述2個(gè)基站中的任意1個(gè)，采用同樣的方法可得到另1條雙曲線方程，兩者的交點(diǎn)即為移動(dòng)標(biāo)簽的位置。TDOA定位算法具體步驟如下。如圖3所示：在坐標(biāo)系OXY中，移動(dòng)標(biāo)簽MS的坐標(biāo)為(x，y)，基站BS1～BS4的坐標(biāo)分別為(x1，y1)～(x4，y4)。根據(jù)TDOA測(cè)距原理可得：

式(1)中：ri表示MS到BSi之間的距離，i=1, 2, 3, 4。對(duì)式(1)兩邊平方可得：

式(2)中：Ki=xi2+yi2。令ri,1為MS到BSi和BS1之間的距離差，可得：

式(3)中：c表示光速(3×108m·s-1)；ti,1表示信號(hào)在MS到BSi和BS1的時(shí)間差值。聯(lián)立式(1)、式(2)和式(3)可得：

式(4)中：xi,1=xi-x1，yi,1=yi-y1。令i=2, 3，帶入式(4)，可得到：

式(6)中：a=(P1P2)TP1P2-1，b=(P1P2)T(P1P3-X1)+(P1P3-X1)T(P1P2)，m=(P1P3-X1)T(P1P3-X1)，X1=[x1,y1]T。由于各基站的坐標(biāo)(x1，y1)～(x4，y4)都為已知，因此將這些數(shù)據(jù)代入上述所有公式即可解出a、b、m。式(6)是一個(gè)關(guān)于r1的一元二次方程，可進(jìn)一步解出r1，最終解出標(biāo)簽的坐標(biāo)(x，y)。

1.2.3 定位誤差分析及誤差修正 UWB信號(hào)在室內(nèi)直線傳播時(shí)，會(huì)受到多徑干擾或障礙物遮擋而產(chǎn)生非視距(NLOS)誤差[22]，因此需掌握定位誤差產(chǎn)生的規(guī)律，并對(duì)誤差進(jìn)行必要的修正。

如圖3所示：在UWB定位區(qū)域中心選定15 m×15 m場(chǎng)地，在場(chǎng)地中畫出1 m間隔的單元格，在坐標(biāo)系OXY中測(cè)量每個(gè)格點(diǎn)坐標(biāo)值。這些測(cè)量值與格點(diǎn)處的精確值之間存在X和Y方向上的誤差。以格點(diǎn)所在的平面為x-y平面，以X和Y方向上的誤差值為z軸，分別構(gòu)建在xyzX和xyzY空間中的誤差值三維散點(diǎn)圖，為發(fā)現(xiàn)誤差分布的規(guī)律，用MATLAB R2012a對(duì)散點(diǎn)圖進(jìn)行二元三次多項(xiàng)式擬合，如圖4所示。得出擬合方程如下式，各項(xiàng)系數(shù)如表1所示。

式(7)中：(x,y)為位置坐標(biāo)的測(cè)量值。本研究2個(gè)擬合方程的決定系數(shù)分別為0.823和0.856，擬合效果較好。則定位修正方程為：

圖4 誤差擬合Figure 4 Error fitting

表1 誤差擬合方程系數(shù)Table 1 Coefficient of error fitting equation

式(8)中：xcorrection和ycorrection分別表示測(cè)量點(diǎn)修正后的橫、縱坐標(biāo)。為驗(yàn)證誤差修正方法的有效性，將測(cè)量值依次帶入式(8)計(jì)算誤差修正后的坐標(biāo)，修正坐標(biāo)與測(cè)量坐標(biāo)對(duì)比如圖5所示，并隨機(jī)選擇了20個(gè)不同的靜態(tài)位置分別采集2 min，100組數(shù)據(jù)，計(jì)算平均值，分別帶入定位修正公式，得到修正后坐標(biāo)和修正前后的平均誤差統(tǒng)計(jì)值。表2顯示：相比修正前坐標(biāo)，修正后坐標(biāo)精度得到明顯的提升。從表3可見：X方向平均誤差減少了0.08 m，定位精度提升了66.7%，Y方向平均誤差減少了0.11 m，定位精度提升了68.8%。因此，修正后的點(diǎn)接近實(shí)際點(diǎn)，驗(yàn)證了定位誤差修正方法的有效性，滿足試驗(yàn)需求。

圖5 靜態(tài)定位測(cè)量及修正圖Figure 5 Static positioning measurement and correction

表2 修正前坐標(biāo)與修正后坐標(biāo)對(duì)比Table 2 Comparison of measured coordinates and corrected coordinates

表3 修正前后平均誤差統(tǒng)計(jì)Table 3 Average error statistics before and after correction

1.3 模糊控制器

本研究設(shè)計(jì)一種帶解析式的模糊控制器，構(gòu)建車體橫向偏差、航向偏差與前輪轉(zhuǎn)向角之間的映射關(guān)系，并利用自調(diào)整函數(shù)動(dòng)態(tài)調(diào)整各輸入變量的權(quán)重，提高生產(chǎn)平臺(tái)在溫室內(nèi)狹窄空間下路徑跟蹤的質(zhì)量。

1.3.1 輸入輸出變量 本研究車體橫向偏差由UWB無線定位系統(tǒng)采集車輛的當(dāng)前絕對(duì)位置獲取，航向偏差由跟蹤航向和陀螺儀采集的當(dāng)前車身航向計(jì)算得到。以車體橫向偏差d和航向偏差θ作為模糊控制器的輸入變量，以前輪轉(zhuǎn)角α作為輸出變量，分別對(duì)3者模糊化。d的基本論域范圍為[-0.6 m，0.6 m]，θ的基本論域范圍為[-60°，60°]，α的論域范圍為[-32°，32°]。由于Mamdani型模糊推理規(guī)則滿足人們思維方式的特點(diǎn)[23]，因此，本研究采用Mamdani型模糊推理。將輸入輸出變量的論域變化范圍都?xì)w一化為[-6，6]之間的變量，得出d的量化因子kd=10，θ的量化因子kθ=0.1，α的比例因子kα=0.17。設(shè)定d、θ和α的正負(fù)號(hào)為：生產(chǎn)平臺(tái)位于導(dǎo)航線左側(cè)時(shí)d為正，右側(cè)為負(fù)；θ逆時(shí)針為正，順時(shí)針為負(fù)；α左轉(zhuǎn)為正，右轉(zhuǎn)為負(fù)。

1.3.2 帶自調(diào)整函數(shù)模糊解析式 設(shè)計(jì)帶有自調(diào)整函數(shù)的模糊解析表達(dá)式[24]。

式(9)中：Α表示前輪轉(zhuǎn)角的模糊變量；D表示橫向偏差的模糊變量；Θ表示航向偏差的模糊變量；S表示論域設(shè)定值，此處為6；n表示自調(diào)整函數(shù)。本研究控制器權(quán)重設(shè)置原則為：生產(chǎn)平臺(tái)在路徑跟蹤過程中，若橫向偏差較大，則增大橫向偏差權(quán)重，使其不偏離路徑較遠(yuǎn)；若橫向偏差較小，則對(duì)航向偏差給予較大權(quán)重，使生產(chǎn)平臺(tái)跟蹤路徑更穩(wěn)定。相比于常規(guī)模糊控制規(guī)則的單一，其提高了控制的靈活性，提升了精度。依據(jù)上述思想，建立自調(diào)整函數(shù)為：

自調(diào)整函數(shù)的形態(tài)根據(jù)p取值不同而改變，如圖6所示。當(dāng)p＜1時(shí)，n隨橫向偏差增加較快，橫向偏差在系統(tǒng)中占據(jù)權(quán)重多；當(dāng)p＞1時(shí)，航向偏差的控制作用加強(qiáng)，隨著p的增大，其作用越強(qiáng)。由于生產(chǎn)平臺(tái)在設(shè)施溫室內(nèi)行駛時(shí)，需頻繁更改當(dāng)前跟蹤路徑，時(shí)常會(huì)出現(xiàn)較大橫向偏差，且道路寬度有限，因此橫向偏差的權(quán)重需始終重視。經(jīng)過MATLAB R2012a仿真，這里自調(diào)整函數(shù)的參數(shù)p=0.8，k=1.1。

圖6 自調(diào)整函數(shù)圖Figure 6 Self-adjusting function graph

2 試驗(yàn)與討論

2.1 試驗(yàn)過程

為了驗(yàn)證本研究方法的有效性，于2019年3-5月，在浙江農(nóng)林大學(xué)室內(nèi)體育館布置場(chǎng)地(圖3)進(jìn)行試驗(yàn)?；九c移動(dòng)標(biāo)簽距離地面的高度均為1.6 m，移動(dòng)標(biāo)簽投影點(diǎn)位于生產(chǎn)平臺(tái)后輪車軸中間。生產(chǎn)平臺(tái)按照0.5 m·s-1的速度行駛，采樣頻率為5 Hz。試驗(yàn)類型分為直線跟蹤與矩形跟蹤2種。同一初始狀態(tài)的試驗(yàn)重復(fù)4次，各數(shù)據(jù)求平均值。

本研究對(duì)偏差的平均值、極值和標(biāo)準(zhǔn)差等進(jìn)行了統(tǒng)計(jì)分析。平均偏差是從試驗(yàn)開始到結(jié)束的橫向偏差均值；最大偏差是在整個(gè)試驗(yàn)過程中橫向偏差最大值；穩(wěn)態(tài)偏差是當(dāng)生產(chǎn)平臺(tái)從試驗(yàn)開始到第1次橫向偏差收斂到0 m的穩(wěn)定點(diǎn)后橫向偏差均值；穩(wěn)定距離是生產(chǎn)平臺(tái)到達(dá)穩(wěn)定點(diǎn)行駛的距離。

試驗(yàn)1：直線路徑跟蹤試驗(yàn)。設(shè)計(jì)4種偏差初始狀態(tài)來考察本研究方法直線路徑的性能，并通過純追蹤法進(jìn)行對(duì)比驗(yàn)證。經(jīng)多次計(jì)算機(jī)仿真，純追蹤模型取最佳前視距離為2 m。如圖7所示。

圖7 直線試驗(yàn)Figure 7 Line test

試驗(yàn)2：矩形路徑跟蹤試驗(yàn)。在溫室作業(yè)時(shí)，農(nóng)機(jī)需頻繁沿著設(shè)施內(nèi)道路直角轉(zhuǎn)向，因此需檢驗(yàn)生產(chǎn)平臺(tái)矩形路徑跟蹤的效果。在試驗(yàn)場(chǎng)地內(nèi)設(shè)計(jì)了15 m×15 m的矩形路線。如圖8所示：生產(chǎn)平臺(tái)出發(fā)的初始位置為A(-7，-7)，初始橫向、航向偏差均為0°，沿順時(shí)針方向行駛。

2.2 數(shù)據(jù)分析

在進(jìn)行直線路徑跟蹤時(shí)，本研究提出的方法在4種初始狀態(tài)下，生產(chǎn)平臺(tái)平均偏差均值依次為28.7、23.8、16.9、20.1 cm，最大偏差依次為179.5、37.1、66.8、46.4 cm，在純追蹤模型中平均偏差均值依次為37.2、35.3、24.4、28.3 cm，最大偏差依次為180.8、51.3、85.6、62.2 cm。本研究方法的平均偏差比純追蹤模型分別提高了22.8%、32.6%、30.7%和28.9%，最大偏差分別提升了0.7%、27.7%、21.9%和25.4%。直線試驗(yàn)時(shí)第1種初始狀態(tài)下偏差明顯大于其他狀態(tài)，是由于初始位置偏離路徑較大，需要進(jìn)行大幅度轉(zhuǎn)向才能逐漸收斂至跟蹤直線。表明相同的環(huán)境下，本研究方法具有更好的路徑跟蹤精度。

采用本研究方法進(jìn)行直線路徑跟蹤時(shí)，平均偏差值的標(biāo)準(zhǔn)差為5.8 cm；而采用純追蹤模型法的平均偏差值的標(biāo)準(zhǔn)差為9.7 cm。標(biāo)準(zhǔn)差越小，說明數(shù)據(jù)離散程度越低，前者的標(biāo)準(zhǔn)差比后者小，因此本研究方法具有更好的跟蹤穩(wěn)定性。

在進(jìn)行直線路徑跟蹤時(shí)，本研究方法4種初始狀態(tài)下的穩(wěn)定距離分別為319.9、35.4、310.1和16.8 cm；在純追蹤模型中，穩(wěn)定距離則為432.1、50.3、250.1和22.4 cm，穩(wěn)定距離提升率分別為25.9%、29.6%、19.3%和25.0%。這是因?yàn)楸狙芯糠椒ㄔ谶M(jìn)行轉(zhuǎn)向角計(jì)算時(shí)，增加航向偏差的權(quán)重動(dòng)態(tài)調(diào)整轉(zhuǎn)向角，使得車體向期望路徑的收斂速度較快。表明相同的試驗(yàn)環(huán)境下，本研究方法具有更好的收斂快速性。

在矩形路徑跟蹤時(shí)，生產(chǎn)平臺(tái)在本研究方法下的平均偏差為14.4 cm，在純追蹤模型中，平均偏差為36.9 cm，平均偏差提升率達(dá)60.9%，能基本滿足作業(yè)需求。在2種方法的矩形試驗(yàn)中，最大偏差都發(fā)生在更換路徑時(shí)，即矩形路徑的轉(zhuǎn)角處。這是由于在更換路徑時(shí)需要做類似于大偏差初始狀態(tài)的直線跟蹤，需要大幅度轉(zhuǎn)向，因此產(chǎn)生大偏差。

3 結(jié)論

采用UWB模塊構(gòu)建無線定位系統(tǒng)獲取生產(chǎn)平臺(tái)的位置信息，并設(shè)計(jì)了15 m×15 m的矩形試驗(yàn)區(qū)域。在試驗(yàn)區(qū)域內(nèi)隨機(jī)測(cè)量一些點(diǎn)，這些點(diǎn)在原X、Y方向定位平均誤差分別為0.12和0.16 m，經(jīng)誤差修正后X、Y方向定位平均誤差分別為0.04和0.05 m。使用帶自調(diào)整函數(shù)的解析式模糊控制器，動(dòng)態(tài)調(diào)整橫向偏差與航向偏差的權(quán)重，對(duì)前輪轉(zhuǎn)角的輸出實(shí)現(xiàn)自適應(yīng)調(diào)整,其中自調(diào)整函數(shù)的參數(shù)p=0.8，k=1.1。在生產(chǎn)平臺(tái)上進(jìn)行了直線和矩形的路徑跟蹤，生產(chǎn)平臺(tái)使用本研究方法的直線跟蹤平均穩(wěn)態(tài)偏差在4.7～6.4 cm，矩形路徑的平均偏差為14.4 cm，能滿足溫室內(nèi)狹窄道路的運(yùn)輸需求。下一步考慮改良生產(chǎn)平臺(tái)的機(jī)械性能，測(cè)試在不同的環(huán)境下UWB的定位精度，以及在不同的路況下生產(chǎn)平臺(tái)路徑跟蹤的平順性。