農用無人車自動導航系統設計

摘要：設計了一套基于STM32控制的施藥農用無人車，在設定路徑和施藥信息后能夠自動導航完成作業，與無人機施藥配合能夠360°無死角地消除蟲害，并且無須人工操作，省時省力。該自動導航設計是基于GPS定位和三軸航向角測定的控制算法，使用SDIO總線操作SD卡記錄航行操作數據，加入實時的自檢代碼以進行路徑修正，加入避障系統以保證人員和車輛安全。系統基于STM32開發板進行開發，擴展性和兼容性強，后續可繼續增添新功能。

關鍵詞：農用無人車；自動導航；STM32；GPS；算法

中圖分類號：S24 文獻標識碼：A 文章編號：2095-1795（2023）03-0035-08

DOI：10.19998/j.cnkl.2095-1795.2023.03.007

0引言

無人機導航長期以來一直是無人機領域的研究熱點，特別是在新能源汽車行業，近年來有了多項突破，無人車自主導航難點主要在于如何獲得高精度地圖和高精度定位。在智能車領域，常見的解決方法是使用人力對地理信息進行精確測繪，輔以高精度定位系統，如GPS+IMU+雷達系統。在農業領域，也可參考該系統設計，但由于預算問題及農業施工環境的復雜性，使得使用人力進行測繪投入加大且移植性變差。試想若一臺施藥無人車，每到一個新田地都要重新進行一次測繪，外加考慮田地環境隨著季節、作物植株產生的多種因素做出調整，成本較高，因此本設計采取了折中方案，通過記錄施工員的動作，在短期內讓無人車能模仿施工員完成巡航施藥的工作，如若遇到環境變化，小幅變化無人車能自動處理，而大幅環境變化則會激活無人車避障系統，交由施工員處理。

本設計基于STM32開發板開發，STM32F107的ARM外設豐富、擴展性強、功耗合理，性能亦可滿足無人車需求，設計借鑒了GPS+IMU+雷達系統，出于成本原因對系統做出了簡化，通過調整算法來彌補定位精度要求，可記錄操作員動作，接受命令后自動復原操作，完成自動巡航施藥。

1導航系統設計

系統基于路徑點存儲信息進行導航，即在自動導航前，需要手動錄入路徑信息，反映到實際操作中就是操作員需要先手動施藥1次。無人車存在多種控制模式，本設計主要探究導航系統的設計，著重于記錄模式和導航模式。

1.1記錄模式

記錄模式主要作用是記錄路徑點和操作信息，以便后續導航時調用。選擇使用SDIO總線讀寫TF卡的方式進行信息的存儲和讀寫。在記錄模式下，啟用定時器，每Is記錄1次無人車行駛數據，這些數據包括無人車上搭載的GPS定位數據、無人車水泵動作和噴灑狀態等，這些數據被加密后寫入TF卡中，每Is寫入512字節，即TF卡的一個扇區，一張普通16 GB的SD卡可以使用卡套變為TF卡，能存儲9h的數據，足夠完成日常工作，如果有更多需求，加大SD卡容量即可。

1.2導航模式

導航模式需要手動進入，其主要功能是提取SD卡中的路徑點信息，并驅動無人車進行巡航、噴灑等工作，具體流程如圖1所示。

通過SDIO總線讀取SD卡，需要設置各種緩存，如SD卡的讀取緩存（sd readbuf），以及解析出來的無人車路徑點經緯度緩存（Lng、Lat）和狀態緩存（reserve state），以便后續計算，還需設置路徑點計數（current_cnt），在每次重啟巡航時，該計數值便清零。

導航模式設計思想：在記錄模式中，每次存儲路徑點都會改變一個計數值（sector cnt），該值可以看作無人車作了多少次行為動作，同時也是往SD卡巾寫人數據的次數，只要約定每次寫入的大小（512字節，詳情參照記錄模式說明），就可以依次讀取解析路徑點，即當current cnt

導航系統要點：獲取高精度的定位、角度信息，其中，定位信息依賴于無人車上搭載的GPS定位系統實現，角度信息則通過三軸數字羅盤獲取，得到的所有數據依靠算法聯系，驅動無人車行動。

避障系統是導航系統不可或缺的一環。在設計中，避障模塊的中斷優先級應最高，防止安全事故的發生，當任意一個避障傳感器觸發時，立即停止無人車移動，并同時停止水泵和噴頭工作。再進入循環，在循環中驅動蜂鳴器發出警告并不停檢查所有傳感器的狀態，只有當所有傳感器都無觸發時，說明障礙物消失了或不再重要，系統才會跳出循環繼續工作。

2模塊設計

選用STM32開發板，導航系統可分為驅動模塊、記錄模塊、讀取解析模塊、算法模塊、巡航控制模塊和避障模塊等。

2.1驅動模塊

驅動模塊將開啟接收無線電并解析的捕獲定時器和PWM驅動定時器，能夠控制小車運動模式和移動。驅動模塊主要用途為驅動小車及小車噴頭、水泵工作，根據需求小車還可加裝機械臂等部件，不過基本驅動方式的核心原理是一致的，以下將介紹無人車的基本驅動。

（1）無人車運動驅動部分一般可使用直流電機驅動，電機旋轉帶動車輪驅動小車移動。常見直流電機有無刷電機和有刷電機兩種，有刷電機操作簡便、控制簡單，但壽命較短，而無刷電機操作稍比之下復雜一些，但壽命較長。本質區別在于無刷勵磁指沒有碳刷換向，通過與轉子同軸的勵磁機輸出勵磁電流，直接通入轉子線圈勵磁，勵磁電流由勵磁調節器控制；有刷勵磁是指發電機無須自帶勵磁機，一般勵磁電源由可控硅直接整流控制，供給轉子線圈，這樣必須由碳刷換向才能通人轉動的轉子線圈。本設計重點不在無人車驅動上，因此選擇有刷電機驅動，操作簡單且成本低廉，將4個電機分兩組，每組通過一個L298N芯片控制。

（2）無人車噴頭/水泵采用舵機驅動：這幾種外置部件的驅動方式一致，都是通過單片機輸出PWM波驅動，但是它們的驅動原理不盡相同，以水泵為例，通過改變PWM波占空比來控制抽水電機的工作速度，占空比越大，則電機的工作功率越大，抽水速度越快，噴頭同理，本質上是對電機輸出功率的控制。

（3）舵機的驅動則不是對電機功率進行控制，而是通過PWM波控制舵機旋轉角度，只有持續正確的PWM波才能讓舵機旋轉到對應角度上，舵機的伺服系統由可變寬度的脈沖來進行控制，控制線是用來傳送脈沖的。脈沖的參數有最小值、最大值和頻率。一般而言，舵機的基準信號都是周期為20 ms，寬度為1.5 ms。這個基準信號定義的位置為中間位置。舵機有最大轉動角度，中間位置的定義就是從這個位置到最大角度與最小角度的量完全一樣。最重要的一點是，不同舵機的最大轉動角度可能不相同，但是其中間位置的脈沖寬度是一定的，為1.5 ms。控制信號如圖2所示。

（4）將驅動控制程序抽象成簡單的命令，如左拐car_left、右拐car_right、停止car_stop等，導航系統直接調用該指令即可完成無人車驅動。

2.2記錄模塊

記錄模塊將初始化SDIO端口，并開啟一個基本定時器，以1s為間距定時采集各傳感器的數據，并通過SDIO總線將目標數據寫入單片機上的SD卡上，以便后續使用。此外若無人車有通訊功能，記錄模塊還可以開啟對應的通訊串口將收集的數據通過網絡發送到云端。記錄模塊主要采集的數據有無人車當前路徑點的經緯度、無人車的工作狀態、各傳感器的采集數據等。

2.3讀取解析模塊

讀取解析模塊通過SDIO總線讀取SD卡上存儲的路徑數據，并解析成可以識別的數據，是完成導航功能的前提，通過不斷解析存儲的路徑點數據，獲得無人車施藥的路線、工作內容，以完成自動施藥工作。

2.4避障模塊

避障模塊將停止無線電捕獲定時器，讓無人車無法接收一般的控制信號，同時強制停止小車的所有動作，避障模塊作用是緊急避障，當無人車認為即將發生碰撞時會觸發中斷自動跳轉該模式并在此循環監測，直到障礙物消失或手動解除中斷。

2.5導航模塊

導航模塊依賴于其他所有模塊，是完成導航作業的核心模塊。在人工使用記錄模塊錄入施藥數據后，通過讀取解析模塊獲得多個路徑點的存儲數據，并擬合成工作路線和作業內容，調用驅動模塊自動完成無人車行駛、工作，而避障模塊則保障自動導航時的人員與機器安全。

3控制代碼設計

以下通過調用無人車控制接口來演示如何實現自動導航。導航系統的解析分為3個部分：第1部分是匹配模式的實現；第2部分是記錄模式的實現；第3部分是導航模式的實現。

3.1匹配模式的實現

多模式控制是設計核心，為實現自動巡航及基本控制，至少需要5種控制模式，即停止模式、正常模式、記錄模式、導航模式和禁止模式，模式之間最主要的區別是外設資源的使用，通過開啟／關閉不同資源，使能/禁用無人車的各個功能，完成不同的應用。

為了理清無人車處于什么模式，需要設置一個vu8型的全局標志位car mode，每次切換模式時更改標志位，以此確定無人車狀態，無人車狀態標記位與模式一一對應。

car mode只是確認無人車“應當”所處的狀態，無人車實際所處的狀態是根據外設的使能情況確定的，因此，還需要設置一個vu8型的全局外設標記位hw switch，每次使能，關閉硬件資源時變化該標志位，如此一來，就能確認無人車狀態。標志位、模式和使能外設對應關系如表1所示。

無人車多模式控制能很好避免控制混亂的情況出現，其核心思想是“什么模式下，做什么事”。在導航模式下就不應該接受無線電控制的干擾，在禁止模式就應該立即停止無人車的各項驅動以保證人員安全。

（1）禁止模式：當避障模塊接收到障礙物信號時，立即關閉無人車的控制信號捕獲定時器，關閉水泵和噴頭等各種硬件驅動，以避免造成人員傷害。

（2）停止模式：僅使能模式控制信號接口，不接受無人車驅動控制信號，避免無人車移動及噴灑，等待正式啟動命令，是個過渡態。

（3）正常模式：手動控制無人車作業的模式，接收無人車驅動控制信號和模式控制信號，能夠更改模式和自由控制無人車。

（4）記錄模式：同樣是手動控制無人車，與正常模式的區別在于使能無人車的各項傳感器，并將采集到的數據通過SDIO總線存儲到SD卡上，以便后續導航時調用。

（5）導航模式：僅使能SDIO總線和模式控制信號，不接收無線電或其他人為控制信號，通過讀取SD卡上儲存的數據及算法模塊，規劃出導航路線并實現自動施藥，導航模式同樣接收模式控制信號，以便操作者隨時暫停導航并手動調整路線，另外，導航模式強烈依賴于避障模塊以保障安全性。

以上便是無人車的5大基本模式，通過模式匹配能明確無人車的工作，并避免硬件資源的浪費。無人車模式切換流程如圖3所示。

3.2記錄模式的實現

記錄模式定時記錄無人車數據，導航需要的基本記錄數據為無人車定位數據及無人車狀態數據，選擇使用高精度GPS模塊進行定位，因此記錄模式需要占用的資源為兩個基本定時器、一個用作定時數據采集、一個用作GPS的數據采集，另外還需要一個串口，用來與GPS模塊進行通信。

3.2.1GPS模塊配置

以北斗GPS模塊ATK-1218-BD為例，GPS模塊為串口通信，初始化GPS模塊前需要初始化串口并配置對應波特率usart3_init（38 400）；檢查GPS模塊是否正常，并設置定位信息更新速度，SkyTra_Cfg_Rate（5），該函數本質上是對串口發送函數的封裝，根據模塊手冊的命令格式撰寫，功能是調整模塊定位數據更新速率為5 Hz，設置完成后，GPS模塊將定時發送采集數據給單片機。

要判定GPS模塊發送的數據是否完成，需要定時器參與，串口設置vu16型標志位USARTX RX STA，標志位第1位確定傳輸是否結束，剩下位置用來記錄收到信息的位置。每當USART IT RXNE標志位置1，即有中斷產生時進行如下判定：若狀態位USARTX RXSTAamp;（1《15））！=0，則傳輸未完成，然后判定數據是否溢出：若USARTX_RX_STA

收到完整數據后，暫時存儲在USART3 RX_buf緩存中，要想得到可讀數據，還需要對數據做一定的處理，建立一個結構體nmea_msg，將解析后的數據存儲到對應的結構體屬性中，GPS模塊采集的數據有可見GPS衛星數、UTC時間、經緯度、GPS狀態、精度因子、海拔高度和地面速率等，封裝一個解析函數將收到的數據拆分并解析，核心思想是運用strstr函數，定位GPS信息分隔符（分隔符符號參見手冊，常見如“$BDGSV”／“SGNVTG”等），返回對應數據片段的指針，按手冊說明解析后存人對應的結構體元素中，如此一來，就能得到無人車的實時坐標數據。

無人車是不需要一直采集數據的，那樣過于費時費力，無人車只需要定時采集定位數據并存入SD卡中，只要采集頻率設置得當，就能通過算法，將離散的定位數據擬合成一條連續的巡航路徑。

3.2.2通過SDIO總線操作SD卡記錄數據

SD卡根據容量m的不同分為3種類型，m≤2 GB的為SDSC；2 GB

SDIO操作SD卡的基本流程：首先初始化SDIO總線，注意初始化時鐘不能gt;400 kHz，配置SDIOInitStructure并設置SDIO總線頻率、數據線數量和硬件流控制使能等基本屬性，然后通過SDIO Send-Command函數向掛載在SDIO總線上的SD卡發送命令操作其寄存器完成讀寫功能，SD卡的寄存器如表2所示。

查閱數據手冊可知，SD命令有4種類型：帶響應的廣播命令（be），命令發送到所有卡，不返回任何響應；有響應的廣播命令（bcr），命令發送到所有卡，接收來自所有卡的響應；尋址命令（ac），命令發送到指定的卡，DAT線無數據傳輸；尋址數據傳輸命令（adte），命令發送到選定卡，DAT有數據傳輸。由于SD卡命令格式較復雜，可封裝為—個SDIO Cmdlnit-TypeDef結構體，將各命令參數分開以便于配置，同樣，SD卡的返回信息也可根據相同思想封裝為一個枚舉類型SD Error。

初始化SD卡流程：SD卡上電，初始化SDIO總線，命令查詢卡是SDSC還是SDHC，并查詢其電壓和配置時鐘（每次查詢后還要等待響應并校驗返回值），配置SDIO通訊模式，完成SD卡初始化。該流程涉及許多SD卡命令，需要對照數據手冊耐心操作。

之后以同樣思想封裝SD卡的寫命令函數SD WriteDisk，傳人參數3個：buf寫數據緩存區；sector扇區地址；cnt扇區個數。寫命令同樣需要反復進行查詢和驗證，首先通過SDIO_CmdlnitTypeDef命令發送CMD13，查詢卡的狀態，并校驗短響應，接著發送CMD24，寫單塊指令，同樣校驗響應，然后使用SDIO_Dataconfig函數配置傳遞數據字長、傳輸方向等基本屬性，之后一邊校驗一邊用SDIO WriteData函數向SD卡寫入數據。

完成了SD卡初始化及SD WriteDisk函數后即可向SD卡寫入數據，之后則是何時寫及寫什么的問題。

首先是何時寫？當于記錄模式時，應定時寫入路徑點數據。當無人車從普通模式進入記錄模式時，在采集傳感器數據的基本定時器中斷函數中有一個if語句，如果是記錄模式，在該代碼塊里調用SD WriteDisk將需求數據寫入SD卡。

配置一個基礎定時器的周期值TIM Period及預分頻值TIM_Prescaler確定多久時間進行一次寫入，系統時鐘為72 MHz的情況下，周期公式為

本次設計設TIM Prescaler=7 199，TIM Period=9999，T=1 s，即1s觸發1次定時器中斷。

對SD卡的讀寫操作以扇區為單位，先通過sprintf語句將關鍵數據按照一定格式（如將不同數據塊之間以$符號隔開，數據塊前加標號等）存入緩存sd writebuf中，然后使用SD_WriteDisk一并寫誒數據，此外需要設置一個屬性sector cnt，記錄當前共使用了多少個扇區，每次調用SD WriteDisk后該值加1，以便后面導航模式調用。

接下來是寫什么的問題，要完成導航功能，應寫入的基本數據有無人車當前經緯度，以及無人車此時動作數據，即無人車各硬件狀態。可設一個全局變量car state記錄無人車硬件動作，每次無人機產生動作時同時改變該狀態位。

3.2.3通過SD卡記錄數據進行導航

得到SD卡的路徑及動作數據后即可通過算法實現自動導航，具體步驟如下。

首先，根據模式匹配思想，需要進入導航模式后才可進行導航，進入導航模式時，會屏蔽掉外部無線電控制信號（關閉對應PWM采集定時器），只接收模式轉換信號，GPS相關的串口和定時器必須打開。

其次，在主循環函數里進行判斷，如果為導航模式，則開始執行導航函數，首先需要從SD卡中讀出并解析數據，利用SDIO總線操作SD卡的基本步驟上文已經說明，讀數據是寫數據的逆操作，封裝一個SD ReadDisk函數，傳入buf讀數據緩存區、sector扇區地址和cnt扇區個數，同樣通過SDIO Dataconfig函數配置數據基本屬性（傳輸方向變為SDIO_Transfer-Dir_ToCard ），然后使用SDIO SendComand發送命令CMD16+設置數據長度為blksize（一字節塊長，即512字節），之后重新配置數據基本屬性：清除DPSM狀態機配置，將傳輸數據字長配置為blksize，發送CMD17+從addr地址處讀取數據，使用查詢模式，輪詢數據（SD POLLING MODE），不斷使用SDIO GetFlagStatus查詢SDIO總線標志位，一旦接收區收到足夠數據，便存入接收緩存buf中，如此便能夠接收設定區塊的存儲數據。

由于存儲時是順序存儲，要還原出保存的路徑數據只需要從區塊0開始，逐一提取數據即可，設置一個變量current cnt標記當前區塊號，導航開始時將該變量設為O，設置一個for循環，當car mode為導航模式且當前區塊號小于記錄區塊號（current_cn

3.3導航模式的實現

導航開始，將SD卡里的數據解析，設Lng current為當前經度值，Lat current為當前緯度值，reserve state為無人車狀態緩存，通過sscanf函數將SD ReadDisk獲得的區塊數據轉化為具體的經緯度、狀態，然后再次調用GPS的GPS_analyse函數，獲得當前位置的經緯度，要完成自動導航，就需要無人車從當前位置移動到存儲位置，已知兩坐標點，還需要無人車的角度即可完成路徑規劃。

獲取無人車當前角度可以用HMC58831數字羅盤實現，該數字羅盤能通過I2C總線與單片機進行通訊，上電后配置I2C接口，設置好主機地址、應答模式、地址模式、總線速度等基本屬性，然后使用I2C Cmd命令開啟I2C總線，封裝總線數據傳輸和讀取函數即可。I2C是常用總線，不進行過多講解，在本項目中，配置好總線后，查閱HMC58831的寄存器手冊，配置指令寄存器00，輸出速率設定為75Hz：12CSAND_BYTE（HMC58831_Address，Ox00，Ox78）；配置模式寄存器02，設為連續測量模式12C SAND BYTE（HMC58831_Address，Ox02，Ox00）；此時只要上電傳感器就會不斷測量無人車磁偏角，以此可以確定無人車角度，傳感器的測量數據存放在角度寄存器03中，有6個字節長度，設立一個緩存數組XYZ Data用來存儲3個軸輸出的數字量，數組中按序號依次存儲X軸數字量的高8位（設序號為Xmsb）；X軸數字量的低8位（設序號為Xlsb）；Z軸數字量的高8位（設序號為Xmsb）；Z軸數字量的低8位（設序號為Zlsb）；Y軸數字量的高8位（設序號為Ymsb）；y軸數字量的低8位（設序號為Ylsb；那么無人車角度（北偏角）的公式即為

atan2（（double）（（int16_t）（（XYZ Data [Ymsb]lt;lt;8）+XYZ Data[Ylsb]）），（double）（（int16_t）（（.YYZ Data[Xmsb]lt;lt;8）+XYZ Data[Xlsb]）））×（180/3.141592 65）+180

封裝成一個HMC58831 Readangle函數，每次調用即可得到無人車此時的角度，設置一個angle變量存儲角度信息。

已知當前經緯度ori_Lng，ori Lat；目標經緯度aim_Lng，aim Lat，無人車存儲動作reserve state，當前角度可用HMC58831 Readangle函數獲得，可寫出導航算法，該算法又由3個主要函數構成。

函數1：Pisfit（Lng_current，Lat_current，Lng，Lat），該函數的作用是判別無人車是否到達目標點，返回一個布爾值，判定方式為計算兩路徑點的直徑dist，dist-sqrt（（ori_Lng-aim_Lng）×（ori_Lng-aim_Lng）+（ori Lat-aim Lat）×（ori Lat-aim Lat））；若dist小于特定值（根據實際GPS精度該特定值需要手動調整），則認為到達目標點，返回1，否則返回0。

函數2：Angelfit（Lng_current，Lat_current，Lng，Lat），該函數思想與函數1一致，作用是校正無人車航向，通過兩目標點算出無人車正確航向角。可用條件判定語句實現，情況分為9種，若經度一致（ori_Lng==aim Lng），根據緯度大小（if（ori Latlt;=aim Lat）？）可判別航向角為0或180°，同理可判定航向角為90或1800的情況。而若是一般情況，則又分4種情況考慮，角度落在在第1、2、3、4象限，以第1象限為例，if（ori_Lng

函數3：car work（reserve state）函數，通過reserve state（無人車存儲狀態）進行噴灑作業等工作，同樣用條件語句實現，通過對狀態位的比對設置無人車硬件狀態，如設置水泵流量，若流量狀態位是reserve state左起第3、4位，則判定if（（sta＆OxOe）！=Ox00），如果水泵有流量，才進行接下來的噴灑動作，校驗水泵的狀態位，根據約定的狀態位調整水泵流量（TIM_SetCompareK函數），其余硬件設置同理。

結合上面3個函數即可實現無人車的自動導航，導航流程總結如下。第1步：驗證是否開啟導航模式，只有導航模式才進入該函數；第2步：判定導航路徑點是否小于存儲路徑點（current_cnt

由于GPS精度不足等問題，在導航中需要不斷進行檢查，已知存儲路徑點是以每1s存儲1次，則需要設置一個導航計時變量msecond，在每次進入循環時以及航向校正循環中，加入一個延時delay ms（10）；每執行一次延時函數msecond+1，循環若正常結束則清零msecond，若循環出了問題，無人車一直到達不了目標點或一直無法匹配正確航向角，則會出現超時，設置一個msecond閾值，在循環中檢測，超出該值強制退出循環，重新定位修正路線，或直接跳過不可用路徑點。

還有一種需要退出循環的情況是操作者手動結束了導航模式進入手動操作，在各個循環中都要加入檢測無人車模式的函數IsExit（），該函數會判定是否處于導航模式，若不處于，則使用無條件轉移語句goto直接跳轉到最外層循環（即控制整個導航模式的循環），并清除current cnt等緩存數據，在外層循環中又會判定是否是導航模式，發現不處于導航模式則會跳過導航程序，操作者即可正常進行手動操作，注意導航模式下雖然無人車不接受操作指令但依然接收模式轉換指令，因此可以隨時退出導航。

在無人導航時，安全是必須首要考慮的，因此必須設置避障模塊并將該模塊優先級調至最高，可以使用多個距離傳感器，在無人車移動時不停檢測周邊物體距離，一旦距離小于危險值便觸發中斷，在中斷內循環檢測障礙物，并驅動蜂鳴器發出警告，若有SIM800C等網絡接口還可用短信或網頁方式提醒操作者，只有障礙物消失或操作者手動解除避障傳感器使能才可離開中斷并繼續工作。

4結束語

設計實現了一個無人車的導航系統，參照空中無人機的設計方式和理念，將路徑點和角度結合，利用STM32平臺完成了導航數據存儲、提取、解析和控制功能，該系統能實現一次施藥、反復利用的特點，讓操作者免于一次次的機械性重復勞動，并具有一定安全保障功能，確保人員作業安全。