基于GPS/ICM 路徑規(guī)劃的智能循跡小車

劉佳明，史亞寧，劉明宇，方增斌，王昊天

（華北理工大學(xué)，河北唐山，063210）

0 引言

智能循跡是基于自動(dòng)引導(dǎo)機(jī)器人系統(tǒng)，用以實(shí)現(xiàn)小車自動(dòng)識(shí)別路線，以及選擇正確的路線。智能循跡小車是一個(gè)運(yùn)用計(jì)算機(jī)控制、傳感器、單片機(jī)、電機(jī)驅(qū)動(dòng)及自動(dòng)控制等技術(shù)來(lái)實(shí)現(xiàn)按照預(yù)先設(shè)定的模式下，不受人為管理時(shí)能夠自動(dòng)實(shí)現(xiàn)循跡導(dǎo)航的高新科技。該技術(shù)已經(jīng)應(yīng)用于汽車制造業(yè)、倉(cāng)儲(chǔ)業(yè)，食品加工業(yè)等多個(gè)行業(yè)，其中汽車制造行業(yè)中的無(wú)人駕駛技術(shù)就體現(xiàn)了此性能。

隨著人工智能技術(shù)和互聯(lián)網(wǎng)科技的發(fā)展，無(wú)人駕駛技術(shù)迅速發(fā)展提升。研究人員預(yù)測(cè)：到2025 年，人類將看到大約800 萬(wàn)輛無(wú)人或半無(wú)人汽車在路上行駛無(wú)人駕駛正在如火如荼地展開(kāi)著，百度和沃爾沃共同研發(fā)L4級(jí)別無(wú)人電動(dòng)車，特斯拉的Autopilot，奧迪的Traffic Jam Pilo等等。世界上最主流的無(wú)人駕駛技術(shù)都是基于激光雷達(dá)、攝像頭視覺(jué)、GPS+IMU 的方案。

本次設(shè)計(jì)的小車很好地應(yīng)用了當(dāng)今社會(huì)的衛(wèi)星定位系統(tǒng)，同時(shí)以MM32 作主控芯片，充分利用最小系統(tǒng)板的便捷之處，將單片機(jī)理論融入智能小車的各個(gè)模塊應(yīng)用，為無(wú)人駕駛領(lǐng)域提供一種新的思路并且有助于推動(dòng)單片機(jī)技術(shù)的發(fā)展[1～3]。

1 智能車控制系統(tǒng)概述

智能車控制系統(tǒng)由測(cè)速模塊、簡(jiǎn)易慣導(dǎo)模塊、PID 控制模塊、遙控控制模塊、MM32 控制單元、電機(jī)控制單元、舵機(jī)控制單元組成[4]。其中，通過(guò)GPS 和ICM 組成的簡(jiǎn)易導(dǎo)航模塊用來(lái)控制小車自身的方位；測(cè)速模塊是編碼器通過(guò)系統(tǒng)定時(shí)中斷實(shí)時(shí)返回電機(jī)轉(zhuǎn)速；PID 控制模塊用來(lái)調(diào)節(jié)電機(jī)轉(zhuǎn)速和舵機(jī)旋轉(zhuǎn)角度；遙控控制模塊通過(guò)連接主板，可以遠(yuǎn)程控制智能車的啟停，以防緊急情況的發(fā)生；MM32 控制單元是整個(gè)控制系統(tǒng)的核心，負(fù)責(zé)系統(tǒng)的數(shù)據(jù)處理，中斷響應(yīng)以及串口通信等功能；電機(jī)控制單元可以通過(guò)PWM 占空比調(diào)節(jié)來(lái)控制智能車的速度；舵機(jī)控制單元也通過(guò)PWM調(diào)節(jié)控制智能車自身角度；設(shè)計(jì)中，GPS 通過(guò)串口中斷實(shí)時(shí)傳回智能車當(dāng)前經(jīng)緯度數(shù)據(jù)，ICM 通過(guò)定時(shí)器中斷實(shí)時(shí)獲得智能車自身方位角信息，見(jiàn)圖1。

圖1 智能車系統(tǒng)構(gòu)架

2 控制系統(tǒng)硬件設(shè)計(jì)

在硬件設(shè)計(jì)中，外設(shè)包括編碼器、GPS、ICM 等均通過(guò)串口通信方式與單片機(jī)進(jìn)行數(shù)據(jù)交換。在本設(shè)計(jì)中，編碼器需要主板提供3.3V 標(biāo)準(zhǔn)電壓，并且通過(guò)PB4 和PB6獲取電機(jī)速度信息，其中PB6 為編碼器方向引腳；電機(jī)通過(guò)PA5 和PA6 引腳和主板連接，PA5 引腳控制電機(jī)轉(zhuǎn)動(dòng)的方向[5]，PA6 引腳控制主板輸出給電機(jī)的PWM；主板通過(guò)PA15 引腳給舵機(jī)輸出PWM，控制舵機(jī)旋轉(zhuǎn)的角度；GPS通過(guò)PC11 和PC10 引腳與主板連接；ICM 通過(guò)PB13 與PB15 引腳與主板連接。本設(shè)計(jì)的硬件原理圖如圖2 所示。

圖2 智能車硬件原理圖

2.1 GPS 模塊

GPS 模塊上有電源供電和通訊接口、復(fù)位按鍵、定位指示燈、天線接口等主要模塊。按下GPS 復(fù)位按鍵后，模塊會(huì)重新冷啟動(dòng)；當(dāng)GPS 定位成功后，GPS 模塊上的PPS等（定位指示燈）會(huì)每秒閃爍一次；本GPS 模塊上的天線為雙頻天線，配以雙頻GPS 使用，可以接收兩個(gè)頻率的衛(wèi)星信號(hào)，用于定位的衛(wèi)星數(shù)量也相較于單頻GPS 有一定的提升，定位也要比單頻穩(wěn)定一些。

GPS 模塊輸出的信息主要有xxGGA、xxGSV、xxRMC、xxGLL、xxGSA 等類型的數(shù)據(jù)。其中最重要的是xxRMC 和xxGGA 兩種類型的數(shù)據(jù)，通過(guò)編程可以對(duì)這兩個(gè)語(yǔ)句進(jìn)行解析，可以得到位置、速度、定位狀態(tài)、高度、時(shí)間、航向角、用于定位的衛(wèi)星數(shù)量等重要信息，本次設(shè)計(jì)中獲取用于定位的衛(wèi)星數(shù)量是為了保證定位精度，在定位衛(wèi)星達(dá)到23 或24 顆時(shí)，定位效果最好；通過(guò)GPS 解析獲得的航向角和位置具有很高的精度，可以作為PID控制模塊的設(shè)定值。

2.2 ICM 模塊

ICM20602六軸陀螺儀模塊可以獲得六個(gè)方向的位置。在本次設(shè)計(jì)中，由于小車在平面運(yùn)動(dòng)，所以只涉及x 軸和y軸的方向角。通過(guò)定時(shí)器中斷實(shí)時(shí)讀取ICM 的角度，定時(shí)器中斷獲取數(shù)據(jù)時(shí)間為5s。通過(guò)解析ICM 獲得的方向角，可以作為PID 實(shí)時(shí)采取的值，在方向環(huán)中，通過(guò)實(shí)時(shí)采集的值與GPS 得到的設(shè)定值作對(duì)比，實(shí)時(shí)控制方向。

2.3 其他功能模塊

其余功能模塊主要還包括編碼器、TFT 屏幕、電機(jī)、舵機(jī)以及蜂鳴器。通過(guò)編碼實(shí)時(shí)獲取電機(jī)轉(zhuǎn)數(shù)，設(shè)定定時(shí)器6中斷時(shí)間為100ms；在編寫(xiě)程序時(shí)，將GPS 解讀到的經(jīng)緯度信息、距離以及方位和ICM 獲取的自身方位角顯示在屏幕上，方便實(shí)時(shí)觀測(cè)信息。

3 控制系統(tǒng)軟件設(shè)計(jì)

系統(tǒng)整體設(shè)計(jì)方案為：主板供電后，首先進(jìn)行初始化程序（包括系統(tǒng)函數(shù)初始化和系統(tǒng)功能模塊初始化），初始化程序完成后，智能車進(jìn)入待機(jī)模式，此時(shí)可以通過(guò)TFT 屏幕檢測(cè)各個(gè)模塊有沒(méi)有正常啟動(dòng)。若所有的模塊確認(rèn)正常啟動(dòng)，則按下遙控按鈕，智能車啟動(dòng)，程序不斷在主函數(shù)中循環(huán)，直至目標(biāo)點(diǎn)迭代到達(dá)最后一個(gè)點(diǎn)，迭代結(jié)束時(shí)，程序運(yùn)行結(jié)束，此時(shí)智能車也完成預(yù)定軌跡的循跡，見(jiàn)圖3。

圖3 程序系統(tǒng)運(yùn)行流程圖

3.1 系統(tǒng)初始化程序

系統(tǒng)上電之后，首先進(jìn)行的是初始化程序，將各模塊初始化啟動(dòng)。首先需要初始化芯片時(shí)鐘，設(shè)定時(shí)鐘工作頻率為120MHz。之后是功能模塊的初始化：(1)電機(jī)和舵機(jī)初始化：初始化主板輸出給電機(jī)和舵機(jī)PWM 的通道，并設(shè)定初始頻率，并輸出初始占空比。(2)串口初始化：需要將需要與GPS 通信的串口進(jìn)行初始化，設(shè)定串口波特率以及發(fā)送接收引腳。(3)外設(shè)模塊初始化：給GPS、ICM、編碼器初始化，使其處于工作狀態(tài)。

3.2 簡(jiǎn)易慣導(dǎo)模塊程序

簡(jiǎn)易慣導(dǎo)系統(tǒng)是GPS 與ICM 的組合，GPS 可以實(shí)時(shí)獲取智能車的位置信息，ICM 可以實(shí)時(shí)獲取自身的方向角。本文設(shè)計(jì)的主控方案是通過(guò)應(yīng)用GPS 加ICM 的組合慣導(dǎo)來(lái)控制智能車的方向。GPS 通過(guò)采集坐標(biāo)經(jīng)緯度計(jì)算初始的方向角，將數(shù)據(jù)保存下來(lái)傳遞給單片機(jī)。ICM 可以獲取小車自身實(shí)時(shí)的方向角，但是大多數(shù)情況下使用ICM 都會(huì)存在零點(diǎn)漂移現(xiàn)象，這時(shí)就需要通過(guò)濾波技術(shù)對(duì)ICM 檢測(cè)到的值進(jìn)行濾波處理[6]，再和單片機(jī)保存的方向角作比較，通過(guò)PID 控制，使得小車的方向角保持在規(guī)定值。

欲使智能車按照規(guī)定的路徑自主導(dǎo)航，首先需要采集定位預(yù)設(shè)軌道位置信息。需要將GPS 得到的數(shù)據(jù)轉(zhuǎn)換為GCJ-02 坐標(biāo)系下的經(jīng)緯度，由于GPS 直接使用的坐標(biāo)系數(shù)WGS-84，所以需要在程序中加入數(shù)據(jù)轉(zhuǎn)換程序，由此得到的坐標(biāo)可以直接在百度地圖中找到，不會(huì)產(chǎn)生較大偏差。ICM實(shí)時(shí)檢測(cè)智能車自身的方位角，并將此數(shù)據(jù)也記錄下來(lái)。

3.3 距離檢測(cè)程序

通過(guò)提前采集軌道位置信息，可以得到兩個(gè)一維數(shù)組。第一個(gè)數(shù)組保存軌道的緯度信息，第二個(gè)數(shù)組保存軌道的經(jīng)度信息。兩個(gè)數(shù)組同序號(hào)對(duì)應(yīng)軌道同一位置。通過(guò)程序內(nèi)的算法，可以得到任意兩點(diǎn)之間的距離。而距離檢測(cè)可以實(shí)現(xiàn)目標(biāo)點(diǎn)的迭代，先計(jì)算好上一次采集點(diǎn)位置和目標(biāo)點(diǎn)位置之間的距離，智能車通過(guò)GPS 實(shí)時(shí)采集行駛位置的經(jīng)緯度，計(jì)算與目標(biāo)點(diǎn)之間的距離。當(dāng)距離到達(dá)要求時(shí)，迭代目標(biāo)點(diǎn)，將前一次的目標(biāo)點(diǎn)變?yōu)榇舜蔚钠鹗键c(diǎn)，下一個(gè)點(diǎn)變?yōu)槟繕?biāo)點(diǎn)。如此重復(fù)直至目標(biāo)點(diǎn)正好為最后一個(gè)點(diǎn)，迭代結(jié)束，完成距離檢測(cè)，見(jiàn)圖4。

圖4 基于距離檢測(cè)的目標(biāo)點(diǎn)迭代

3.4 PID 控制程序

PID 控制主要是對(duì)系統(tǒng)偏差進(jìn)行比例、積分、微分三種操作并線性組合成控制量，以減小系統(tǒng)誤差，提高系統(tǒng)響應(yīng)速度和響應(yīng)效果。經(jīng)典PID 控制又分為位置型PID 和增量式PID。

（1）比例控制：控制信號(hào)與誤差成正比（誤差越大，控制信號(hào)越大，控制量越強(qiáng)，改變現(xiàn)狀的能力也就越強(qiáng)），故當(dāng)目標(biāo)距離期望較遠(yuǎn)時(shí)，誤差較大，控制量也較大，可以使得目標(biāo)快速接近期望，但問(wèn)題是當(dāng)目標(biāo)接近期望時(shí)誤差會(huì)變得很小，此時(shí)控制量也會(huì)很小，這又使得目標(biāo)只能處于接近但始終不能真正達(dá)到期望的情況。

（2）積分控制：累積誤差并直接加到控制量上。當(dāng)穩(wěn)態(tài)誤差存在時(shí)，誤差積分就會(huì)變大，同時(shí)控制量就會(huì)增大。

（3）微分控制：計(jì)算當(dāng)前誤差的變化率并加至控制量。當(dāng)外界發(fā)生一個(gè)很強(qiáng)的干擾時(shí)，目標(biāo)受影響程度就會(huì)很大，這時(shí)就會(huì)很快的偏離期望，而微分控制的應(yīng)用就是讓目標(biāo)偏離期望后給它一個(gè)很大的控制量，讓其也能很快的返回穩(wěn)態(tài)。

3.4.1 電機(jī)PID 控制程序

電機(jī)PID 控制中，需要預(yù)先設(shè)定電機(jī)工作的轉(zhuǎn)速，通過(guò)測(cè)設(shè)找到合適的編碼器轉(zhuǎn)數(shù)來(lái)達(dá)到控制目的。在控制中，需要電機(jī)工作在一個(gè)穩(wěn)定的轉(zhuǎn)速上，所以PID 的設(shè)定值為設(shè)定轉(zhuǎn)速對(duì)應(yīng)的編碼器轉(zhuǎn)數(shù)，然后通過(guò)定時(shí)器中斷實(shí)時(shí)返回的編碼器轉(zhuǎn)數(shù)，設(shè)定值與返回值作差，將此偏差信號(hào)輸入到PID 中，通過(guò)PID 控制實(shí)現(xiàn)電機(jī)轉(zhuǎn)速控制。

3.4.2 舵機(jī)PID 控制程序

通過(guò)慣導(dǎo)系統(tǒng)，可以獲得兩個(gè)方向角，一個(gè)是通過(guò)GPS 得到的兩個(gè)目標(biāo)點(diǎn)之間的角度，另一個(gè)是通過(guò)ICM 實(shí)時(shí)檢測(cè)得到的角度[7]。將通過(guò)GPS 得到的數(shù)據(jù)計(jì)算得到的兩個(gè)目標(biāo)點(diǎn)之間的角度作為預(yù)設(shè)值，ICM 角度作為反饋值，作PID 控制，控制舵機(jī)旋轉(zhuǎn)相應(yīng)的角度，在舵機(jī)控制中，采用增量型PID 比位置型PID 有更好的控制效果，這是由于舵機(jī)旋轉(zhuǎn)角度是一個(gè)較小的值，而且每次調(diào)節(jié)過(guò)程中需要調(diào)節(jié)的角度也較小，是一個(gè)平緩的調(diào)節(jié)，所以增量型PID 調(diào)節(jié)有更好的效果，見(jiàn)圖5。

圖5 舵機(jī)PID 控制原理圖

4 總結(jié)

經(jīng)過(guò)對(duì)MM32 控制下智能車的調(diào)試與改進(jìn)，該智能車可實(shí)現(xiàn)自動(dòng)循跡，速度調(diào)控、導(dǎo)航定位等一系列智能化功能。本設(shè)計(jì)可以為無(wú)人駕駛領(lǐng)域甚至機(jī)器人領(lǐng)域提供一個(gè)全新的思路，在簡(jiǎn)易組合慣導(dǎo)的作用下，智能車可以實(shí)現(xiàn)自循跡，良好的完成控制任務(wù)。