基于無線定位的溫室生產平臺自動導航

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

（浙江農林大學 工程學院，浙江 杭州 311300）

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

1 材料與方法

1.1 試驗材料

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

圖1 生產平臺Figure 1 Prodsuction platform

1.2 定位方法

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

圖2 控制系統原理圖Figure 2 Block diagram of control system

圖3 試驗區域Figure 3 Test area

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

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

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

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

圖4 誤差擬合Figure 4 Error fitting

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

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

圖5 靜態定位測量及修正圖Figure 5 Static positioning measurement and correction

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

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

1.3 模糊控制器

本研究設計一種帶解析式的模糊控制器，構建車體橫向偏差、航向偏差與前輪轉向角之間的映射關系，并利用自調整函數動態調整各輸入變量的權重，提高生產平臺在溫室內狹窄空間下路徑跟蹤的質量。

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

1.3.2 帶自調整函數模糊解析式 設計帶有自調整函數的模糊解析表達式[24]。

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

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

圖6 自調整函數圖Figure 6 Self-adjusting function graph

2 試驗與討論

2.1 試驗過程

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

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

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

圖7 直線試驗Figure 7 Line test

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

2.2 數據分析

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

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

在進行直線路徑跟蹤時，本研究方法4種初始狀態下的穩定距離分別為319.9、35.4、310.1和16.8 cm；在純追蹤模型中，穩定距離則為432.1、50.3、250.1和22.4 cm，穩定距離提升率分別為25.9%、29.6%、19.3%和25.0%。這是因為本研究方法在進行轉向角計算時，增加航向偏差的權重動態調整轉向角，使得車體向期望路徑的收斂速度較快。表明相同的試驗環境下，本研究方法具有更好的收斂快速性。

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

3 結論

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