基于無線定位的溫室生產(chǎn)平臺自動導航

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

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

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

1 材料與方法

1.1 試驗材料

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

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

1.2 定位方法

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

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

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

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

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

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

式(3)中：c表示光速(3×108m·s-1)；ti,1表示信號在MS到BSi和BS1的時間差值。聯(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。由于各基站的坐標(x1，y1)～(x4，y4)都為已知，因此將這些數(shù)據(jù)代入上述所有公式即可解出a、b、m。式(6)是一個關(guān)于r1的一元二次方程，可進一步解出r1，最終解出標簽的坐標(x，y)。

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

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

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

圖4 誤差擬合Figure 4 Error fitting

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

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

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

表2 修正前坐標與修正后坐標對比Table 2 Comparison of measured coordinates and corrected coordinates

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

1.3 模糊控制器

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

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

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

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

自調(diào)整函數(shù)的形態(tài)根據(jù)p取值不同而改變，如圖6所示。當p＜1時，n隨橫向偏差增加較快，橫向偏差在系統(tǒng)中占據(jù)權(quán)重多；當p＞1時，航向偏差的控制作用加強，隨著p的增大，其作用越強。由于生產(chǎn)平臺在設(shè)施溫室內(nèi)行駛時，需頻繁更改當前跟蹤路徑，時常會出現(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 試驗與討論

2.1 試驗過程

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

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

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

圖7 直線試驗Figure 7 Line test

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

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

在進行直線路徑跟蹤時，本研究提出的方法在4種初始狀態(tài)下，生產(chǎn)平臺平均偏差均值依次為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%。直線試驗時第1種初始狀態(tài)下偏差明顯大于其他狀態(tài)，是由于初始位置偏離路徑較大，需要進行大幅度轉(zhuǎn)向才能逐漸收斂至跟蹤直線。表明相同的環(huán)境下，本研究方法具有更好的路徑跟蹤精度。

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

在進行直線路徑跟蹤時，本研究方法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%。這是因為本研究方法在進行轉(zhuǎn)向角計算時，增加航向偏差的權(quán)重動態(tài)調(diào)整轉(zhuǎn)向角，使得車體向期望路徑的收斂速度較快。表明相同的試驗環(huán)境下，本研究方法具有更好的收斂快速性。

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

3 結(jié)論

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