農(nóng)用無人車自動(dòng)導(dǎo)航系統(tǒng)設(shè)計(jì)

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

關(guān)鍵詞：農(nóng)用無人車；自動(dòng)導(dǎo)航；STM32；GPS；算法

中圖分類號：S24 文獻(xiàn)標(biāo)識碼：A 文章編號：2095-1795（2023）03-0035-08

DOI：10.19998/j.cnkl.2095-1795.2023.03.007

0引言

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

本設(shè)計(jì)基于STM32開發(fā)板開發(fā)，STM32F107的ARM外設(shè)豐富、擴(kuò)展性強(qiáng)、功耗合理，性能亦可滿足無人車需求，設(shè)計(jì)借鑒了GPS+IMU+雷達(dá)系統(tǒng)，出于成本原因?qū)ο到y(tǒng)做出了簡化，通過調(diào)整算法來彌補(bǔ)定位精度要求，可記錄操作員動(dòng)作，接受命令后自動(dòng)復(fù)原操作，完成自動(dòng)巡航施藥。

1導(dǎo)航系統(tǒng)設(shè)計(jì)

系統(tǒng)基于路徑點(diǎn)存儲信息進(jìn)行導(dǎo)航，即在自動(dòng)導(dǎo)航前，需要手動(dòng)錄入路徑信息，反映到實(shí)際操作中就是操作員需要先手動(dòng)施藥1次。無人車存在多種控制模式，本設(shè)計(jì)主要探究導(dǎo)航系統(tǒng)的設(shè)計(jì)，著重于記錄模式和導(dǎo)航模式。

1.1記錄模式

記錄模式主要作用是記錄路徑點(diǎn)和操作信息，以便后續(xù)導(dǎo)航時(shí)調(diào)用。選擇使用SDIO總線讀寫TF卡的方式進(jìn)行信息的存儲和讀寫。在記錄模式下，啟用定時(shí)器，每Is記錄1次無人車行駛數(shù)據(jù)，這些數(shù)據(jù)包括無人車上搭載的GPS定位數(shù)據(jù)、無人車水泵動(dòng)作和噴灑狀態(tài)等，這些數(shù)據(jù)被加密后寫入TF卡中，每Is寫入512字節(jié)，即TF卡的一個(gè)扇區(qū)，一張普通16 GB的SD卡可以使用卡套變?yōu)門F卡，能存儲9h的數(shù)據(jù)，足夠完成日常工作，如果有更多需求，加大SD卡容量即可。

1.2導(dǎo)航模式

導(dǎo)航模式需要手動(dòng)進(jìn)入，其主要功能是提取SD卡中的路徑點(diǎn)信息，并驅(qū)動(dòng)無人車進(jìn)行巡航、噴灑等工作，具體流程如圖1所示。

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

導(dǎo)航模式設(shè)計(jì)思想：在記錄模式中，每次存儲路徑點(diǎn)都會改變一個(gè)計(jì)數(shù)值（sector cnt），該值可以看作無人車作了多少次行為動(dòng)作，同時(shí)也是往SD卡巾寫人數(shù)據(jù)的次數(shù)，只要約定每次寫入的大?。?12字節(jié)，詳情參照記錄模式說明），就可以依次讀取解析路徑點(diǎn)，即當(dāng)current cnt

導(dǎo)航系統(tǒng)要點(diǎn)：獲取高精度的定位、角度信息，其中，定位信息依賴于無人車上搭載的GPS定位系統(tǒng)實(shí)現(xiàn)，角度信息則通過三軸數(shù)字羅盤獲取，得到的所有數(shù)據(jù)依靠算法聯(lián)系，驅(qū)動(dòng)無人車行動(dòng)。

避障系統(tǒng)是導(dǎo)航系統(tǒng)不可或缺的一環(huán)。在設(shè)計(jì)中，避障模塊的中斷優(yōu)先級應(yīng)最高，防止安全事故的發(fā)生，當(dāng)任意一個(gè)避障傳感器觸發(fā)時(shí)，立即停止無人車移動(dòng)，并同時(shí)停止水泵和噴頭工作。再進(jìn)入循環(huán)，在循環(huán)中驅(qū)動(dòng)蜂鳴器發(fā)出警告并不停檢查所有傳感器的狀態(tài)，只有當(dāng)所有傳感器都無觸發(fā)時(shí)，說明障礙物消失了或不再重要，系統(tǒng)才會跳出循環(huán)繼續(xù)工作。

2模塊設(shè)計(jì)

選用STM32開發(fā)板，導(dǎo)航系統(tǒng)可分為驅(qū)動(dòng)模塊、記錄模塊、讀取解析模塊、算法模塊、巡航控制模塊和避障模塊等。

2.1驅(qū)動(dòng)模塊

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

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

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

（3）舵機(jī)的驅(qū)動(dòng)則不是對電機(jī)功率進(jìn)行控制，而是通過PWM波控制舵機(jī)旋轉(zhuǎn)角度，只有持續(xù)正確的PWM波才能讓舵機(jī)旋轉(zhuǎn)到對應(yīng)角度上，舵機(jī)的伺服系統(tǒng)由可變寬度的脈沖來進(jìn)行控制，控制線是用來傳送脈沖的。脈沖的參數(shù)有最小值、最大值和頻率。一般而言，舵機(jī)的基準(zhǔn)信號都是周期為20 ms，寬度為1.5 ms。這個(gè)基準(zhǔn)信號定義的位置為中間位置。舵機(jī)有最大轉(zhuǎn)動(dòng)角度，中間位置的定義就是從這個(gè)位置到最大角度與最小角度的量完全一樣。最重要的一點(diǎn)是，不同舵機(jī)的最大轉(zhuǎn)動(dòng)角度可能不相同，但是其中間位置的脈沖寬度是一定的，為1.5 ms?？刂菩盘柸鐖D2所示。

（4）將驅(qū)動(dòng)控制程序抽象成簡單的命令，如左拐car_left、右拐car_right、停止car_stop等，導(dǎo)航系統(tǒng)直接調(diào)用該指令即可完成無人車驅(qū)動(dòng)。

2.2記錄模塊

記錄模塊將初始化SDIO端口，并開啟一個(gè)基本定時(shí)器，以1s為間距定時(shí)采集各傳感器的數(shù)據(jù)，并通過SDIO總線將目標(biāo)數(shù)據(jù)寫入單片機(jī)上的SD卡上，以便后續(xù)使用。此外若無人車有通訊功能，記錄模塊還可以開啟對應(yīng)的通訊串口將收集的數(shù)據(jù)通過網(wǎng)絡(luò)發(fā)送到云端。記錄模塊主要采集的數(shù)據(jù)有無人車當(dāng)前路徑點(diǎn)的經(jīng)緯度、無人車的工作狀態(tài)、各傳感器的采集數(shù)據(jù)等。

2.3讀取解析模塊

讀取解析模塊通過SDIO總線讀取SD卡上存儲的路徑數(shù)據(jù)，并解析成可以識別的數(shù)據(jù)，是完成導(dǎo)航功能的前提，通過不斷解析存儲的路徑點(diǎn)數(shù)據(jù)，獲得無人車施藥的路線、工作內(nèi)容，以完成自動(dòng)施藥工作。

2.4避障模塊

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

2.5導(dǎo)航模塊

導(dǎo)航模塊依賴于其他所有模塊，是完成導(dǎo)航作業(yè)的核心模塊。在人工使用記錄模塊錄入施藥數(shù)據(jù)后，通過讀取解析模塊獲得多個(gè)路徑點(diǎn)的存儲數(shù)據(jù)，并擬合成工作路線和作業(yè)內(nèi)容，調(diào)用驅(qū)動(dòng)模塊自動(dòng)完成無人車行駛、工作，而避障模塊則保障自動(dòng)導(dǎo)航時(shí)的人員與機(jī)器安全。

3控制代碼設(shè)計(jì)

以下通過調(diào)用無人車控制接口來演示如何實(shí)現(xiàn)自動(dòng)導(dǎo)航。導(dǎo)航系統(tǒng)的解析分為3個(gè)部分：第1部分是匹配模式的實(shí)現(xiàn)；第2部分是記錄模式的實(shí)現(xiàn)；第3部分是導(dǎo)航模式的實(shí)現(xiàn)。

3.1匹配模式的實(shí)現(xiàn)

多模式控制是設(shè)計(jì)核心，為實(shí)現(xiàn)自動(dòng)巡航及基本控制，至少需要5種控制模式，即停止模式、正常模式、記錄模式、導(dǎo)航模式和禁止模式，模式之間最主要的區(qū)別是外設(shè)資源的使用，通過開啟／關(guān)閉不同資源，使能/禁用無人車的各個(gè)功能，完成不同的應(yīng)用。

為了理清無人車處于什么模式，需要設(shè)置一個(gè)vu8型的全局標(biāo)志位car mode，每次切換模式時(shí)更改標(biāo)志位，以此確定無人車狀態(tài)，無人車狀態(tài)標(biāo)記位與模式一一對應(yīng)。

car mode只是確認(rèn)無人車“應(yīng)當(dāng)”所處的狀態(tài)，無人車實(shí)際所處的狀態(tài)是根據(jù)外設(shè)的使能情況確定的，因此，還需要設(shè)置一個(gè)vu8型的全局外設(shè)標(biāo)記位hw switch，每次使能，關(guān)閉硬件資源時(shí)變化該標(biāo)志位，如此一來，就能確認(rèn)無人車狀態(tài)。標(biāo)志位、模式和使能外設(shè)對應(yīng)關(guān)系如表1所示。

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

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

（2）停止模式：僅使能模式控制信號接口，不接受無人車驅(qū)動(dòng)控制信號，避免無人車移動(dòng)及噴灑，等待正式啟動(dòng)命令，是個(gè)過渡態(tài)。

（3）正常模式：手動(dòng)控制無人車作業(yè)的模式，接收無人車驅(qū)動(dòng)控制信號和模式控制信號，能夠更改模式和自由控制無人車。

（4）記錄模式：同樣是手動(dòng)控制無人車，與正常模式的區(qū)別在于使能無人車的各項(xiàng)傳感器，并將采集到的數(shù)據(jù)通過SDIO總線存儲到SD卡上，以便后續(xù)導(dǎo)航時(shí)調(diào)用。

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

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

3.2記錄模式的實(shí)現(xiàn)

記錄模式定時(shí)記錄無人車數(shù)據(jù)，導(dǎo)航需要的基本記錄數(shù)據(jù)為無人車定位數(shù)據(jù)及無人車狀態(tài)數(shù)據(jù)，選擇使用高精度GPS模塊進(jìn)行定位，因此記錄模式需要占用的資源為兩個(gè)基本定時(shí)器、一個(gè)用作定時(shí)數(shù)據(jù)采集、一個(gè)用作GPS的數(shù)據(jù)采集，另外還需要一個(gè)串口，用來與GPS模塊進(jìn)行通信。

3.2.1GPS模塊配置

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

要判定GPS模塊發(fā)送的數(shù)據(jù)是否完成，需要定時(shí)器參與，串口設(shè)置vu16型標(biāo)志位USARTX RX STA，標(biāo)志位第1位確定傳輸是否結(jié)束，剩下位置用來記錄收到信息的位置。每當(dāng)USART IT RXNE標(biāo)志位置1，即有中斷產(chǎn)生時(shí)進(jìn)行如下判定：若狀態(tài)位USARTX RXSTAamp;（1《15））！=0，則傳輸未完成，然后判定數(shù)據(jù)是否溢出：若USARTX_RX_STA

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

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

3.2.2通過SDIO總線操作SD卡記錄數(shù)據(jù)

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

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

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

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

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

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

首先是何時(shí)寫？當(dāng)于記錄模式時(shí)，應(yīng)定時(shí)寫入路徑點(diǎn)數(shù)據(jù)。當(dāng)無人車從普通模式進(jìn)入記錄模式時(shí)，在采集傳感器數(shù)據(jù)的基本定時(shí)器中斷函數(shù)中有一個(gè)if語句，如果是記錄模式，在該代碼塊里調(diào)用SD WriteDisk將需求數(shù)據(jù)寫入SD卡。

配置一個(gè)基礎(chǔ)定時(shí)器的周期值TIM Period及預(yù)分頻值TIM_Prescaler確定多久時(shí)間進(jìn)行一次寫入，系統(tǒng)時(shí)鐘為72 MHz的情況下，周期公式為

本次設(shè)計(jì)設(shè)TIM Prescaler=7 199，TIM Period=9999，T=1 s，即1s觸發(fā)1次定時(shí)器中斷。

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

接下來是寫什么的問題，要完成導(dǎo)航功能，應(yīng)寫入的基本數(shù)據(jù)有無人車當(dāng)前經(jīng)緯度，以及無人車此時(shí)動(dòng)作數(shù)據(jù)，即無人車各硬件狀態(tài)?？稍O(shè)一個(gè)全局變量car state記錄無人車硬件動(dòng)作，每次無人機(jī)產(chǎn)生動(dòng)作時(shí)同時(shí)改變該狀態(tài)位。

3.2.3通過SD卡記錄數(shù)據(jù)進(jìn)行導(dǎo)航

得到SD卡的路徑及動(dòng)作數(shù)據(jù)后即可通過算法實(shí)現(xiàn)自動(dòng)導(dǎo)航，具體步驟如下。

首先，根據(jù)模式匹配思想，需要進(jìn)入導(dǎo)航模式后才可進(jìn)行導(dǎo)航，進(jìn)入導(dǎo)航模式時(shí)，會屏蔽掉外部無線電控制信號（關(guān)閉對應(yīng)PWM采集定時(shí)器），只接收模式轉(zhuǎn)換信號，GPS相關(guān)的串口和定時(shí)器必須打開。

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

由于存儲時(shí)是順序存儲，要還原出保存的路徑數(shù)據(jù)只需要從區(qū)塊0開始，逐一提取數(shù)據(jù)即可，設(shè)置一個(gè)變量current cnt標(biāo)記當(dāng)前區(qū)塊號，導(dǎo)航開始時(shí)將該變量設(shè)為O，設(shè)置一個(gè)for循環(huán)，當(dāng)car mode為導(dǎo)航模式且當(dāng)前區(qū)塊號小于記錄區(qū)塊號（current_cn

3.3導(dǎo)航模式的實(shí)現(xiàn)

導(dǎo)航開始，將SD卡里的數(shù)據(jù)解析，設(shè)Lng current為當(dāng)前經(jīng)度值，Lat current為當(dāng)前緯度值，reserve state為無人車狀態(tài)緩存，通過sscanf函數(shù)將SD ReadDisk獲得的區(qū)塊數(shù)據(jù)轉(zhuǎn)化為具體的經(jīng)緯度、狀態(tài)，然后再次調(diào)用GPS的GPS_analyse函數(shù)，獲得當(dāng)前位置的經(jīng)緯度，要完成自動(dòng)導(dǎo)航，就需要無人車從當(dāng)前位置移動(dòng)到存儲位置，已知兩坐標(biāo)點(diǎn)，還需要無人車的角度即可完成路徑規(guī)劃。

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

封裝成一個(gè)HMC58831 Readangle函數(shù)，每次調(diào)用即可得到無人車此時(shí)的角度，設(shè)置一個(gè)angle變量存儲角度信息。

已知當(dāng)前經(jīng)緯度ori_Lng，ori Lat；目標(biāo)經(jīng)緯度aim_Lng，aim Lat，無人車存儲動(dòng)作reserve state，當(dāng)前角度可用HMC58831 Readangle函數(shù)獲得，可寫出導(dǎo)航算法，該算法又由3個(gè)主要函數(shù)構(gòu)成。

函數(shù)1：Pisfit（Lng_current，Lat_current，Lng，Lat），該函數(shù)的作用是判別無人車是否到達(dá)目標(biāo)點(diǎn)，返回一個(gè)布爾值，判定方式為計(jì)算兩路徑點(diǎn)的直徑dist，dist-sqrt（（ori_Lng-aim_Lng）×（ori_Lng-aim_Lng）+（ori Lat-aim Lat）×（ori Lat-aim Lat））；若dist小于特定值（根據(jù)實(shí)際GPS精度該特定值需要手動(dòng)調(diào)整），則認(rèn)為到達(dá)目標(biāo)點(diǎn)，返回1，否則返回0。

函數(shù)2：Angelfit（Lng_current，Lat_current，Lng，Lat），該函數(shù)思想與函數(shù)1一致，作用是校正無人車航向，通過兩目標(biāo)點(diǎn)算出無人車正確航向角?？捎脳l件判定語句實(shí)現(xiàn)，情況分為9種，若經(jīng)度一致（ori_Lng==aim Lng），根據(jù)緯度大?。╥f（ori Latlt;=aim Lat）？）可判別航向角為0或180°，同理可判定航向角為90或1800的情況。而若是一般情況，則又分4種情況考慮，角度落在在第1、2、3、4象限，以第1象限為例，if（ori_Lng

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

結(jié)合上面3個(gè)函數(shù)即可實(shí)現(xiàn)無人車的自動(dòng)導(dǎo)航，導(dǎo)航流程總結(jié)如下。第1步：驗(yàn)證是否開啟導(dǎo)航模式，只有導(dǎo)航模式才進(jìn)入該函數(shù)；第2步：判定導(dǎo)航路徑點(diǎn)是否小于存儲路徑點(diǎn)（current_cnt

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

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

在無人導(dǎo)航時(shí)，安全是必須首要考慮的，因此必須設(shè)置避障模塊并將該模塊優(yōu)先級調(diào)至最高，可以使用多個(gè)距離傳感器，在無人車移動(dòng)時(shí)不停檢測周邊物體距離，一旦距離小于危險(xiǎn)值便觸發(fā)中斷，在中斷內(nèi)循環(huán)檢測障礙物，并驅(qū)動(dòng)蜂鳴器發(fā)出警告，若有SIM800C等網(wǎng)絡(luò)接口還可用短信或網(wǎng)頁方式提醒操作者，只有障礙物消失或操作者手動(dòng)解除避障傳感器使能才可離開中斷并繼續(xù)工作。

4結(jié)束語

設(shè)計(jì)實(shí)現(xiàn)了一個(gè)無人車的導(dǎo)航系統(tǒng)，參照空中無人機(jī)的設(shè)計(jì)方式和理念，將路徑點(diǎn)和角度結(jié)合，利用STM32平臺完成了導(dǎo)航數(shù)據(jù)存儲、提取、解析和控制功能，該系統(tǒng)能實(shí)現(xiàn)一次施藥、反復(fù)利用的特點(diǎn)，讓操作者免于一次次的機(jī)械性重復(fù)勞動(dòng)，并具有一定安全保障功能，確保人員作業(yè)安全。