基于GPS定位的自主運(yùn)動(dòng)導(dǎo)航系統(tǒng)研究

丁祖磊，蔣天澤，溫秀平，曾 靜

（南京工程學(xué)院 創(chuàng)新創(chuàng)業(yè)學(xué)院，南京 211167）

汽車(chē)是20 世紀(jì)最偉大的發(fā)明之一， 它讓人們的出行更加的便捷和自由，使人類(lèi)的生活方式發(fā)生了巨大的變化[1]。21 世紀(jì)以來(lái)，科學(xué)家和工程師們將高新電子電路、機(jī)器視覺(jué)、人工智能等學(xué)科技術(shù)應(yīng)用到車(chē)輛工程上。 近幾年，許多汽車(chē)制造廠商陸續(xù)推出了具有自動(dòng)入庫(kù)、輔助駕駛、自動(dòng)泊車(chē)等功能的車(chē)輛[2]。 自主導(dǎo)航最開(kāi)始是由谷歌在2009年公開(kāi)宣布， 最早運(yùn)用于汽車(chē)上， 開(kāi)發(fā)自動(dòng)導(dǎo)航汽車(chē)，國(guó)內(nèi)，百度最早于2017年發(fā)布“Appllo”計(jì)劃。 表面上，自動(dòng)駕駛車(chē)輛是以工業(yè)量產(chǎn)的汽車(chē)為基礎(chǔ)進(jìn)行改裝的，而本質(zhì)上，是通過(guò)傳感器感知外部環(huán)境，并通過(guò)計(jì)算機(jī)控制其自主運(yùn)動(dòng)，所以自動(dòng)駕駛車(chē)輛可以歸類(lèi)于機(jī)器人的行列。 自動(dòng)駕駛技術(shù)被國(guó)際自動(dòng)機(jī)工程師學(xué)會(huì)（SAE）分為L(zhǎng)0～L5 共計(jì)6 個(gè)等級(jí)（SAE，2014）[3]。 無(wú)人車(chē)可以分為兩大類(lèi)：室內(nèi)無(wú)人車(chē)和室外無(wú)人車(chē)。 其中，室內(nèi)無(wú)人車(chē)包括送餐機(jī)器人、掃地機(jī)器人等，室外無(wú)人車(chē)包括谷歌無(wú)人車(chē)、無(wú)人偵探車(chē)、百度無(wú)人車(chē)等；室外無(wú)人車(chē)大多以激光雷達(dá)為主，通過(guò)點(diǎn)云來(lái)建立周邊環(huán)境的3D 模型，比如可以檢測(cè)出樹(shù)木、車(chē)輛、路沿、行人等細(xì)節(jié)[4]，再與GPS 定位組合，從而實(shí)現(xiàn)車(chē)輛所需的高精度定位[5]。 因此，室外無(wú)人車(chē)可以結(jié)合北斗或者GPS 導(dǎo)航系統(tǒng)實(shí)現(xiàn)室外定位，然后使用激光雷達(dá)實(shí)現(xiàn)車(chē)庫(kù)、偏僻地區(qū)的地圖構(gòu)建與導(dǎo)航。 截至目前，已有微軟、博世、大陸、TomTom、Grab 等全球近50 家企業(yè)加入Apollo平臺(tái)[6]。 2019年，百度發(fā)布了Apollo 3.5 版本，它是一款可以應(yīng)用于復(fù)雜的城市道路中的自動(dòng)駕駛[7]，本文基于GPS 定位和高德地圖，設(shè)計(jì)了一個(gè)適合于小型室外機(jī)器人本體、助盲導(dǎo)航等多個(gè)應(yīng)用場(chǎng)景的導(dǎo)航系統(tǒng)。

1 系統(tǒng)整體框架

1.1 模塊關(guān)系圖

本系統(tǒng)主要由GPS 定位模塊、 語(yǔ)音交互模塊、路徑規(guī)劃模塊、運(yùn)動(dòng)控制模塊4 部分組成，如圖1所示。

圖1 模塊關(guān)系圖Fig.1 Module diagram

GPS 模塊獲取當(dāng)前位置的經(jīng)緯度信息，語(yǔ)音模塊識(shí)別用戶(hù)發(fā)出的指令，樹(shù)莓派整合數(shù)據(jù)鏈接高德地圖進(jìn)行路徑規(guī)劃獲取數(shù)據(jù)，將數(shù)據(jù)處理后發(fā)送至運(yùn)動(dòng)控制模塊進(jìn)行控制。

1.2 系統(tǒng)工作框圖

在系統(tǒng)工作時(shí)，首先通過(guò)語(yǔ)音識(shí)別識(shí)別出用戶(hù)想要到達(dá)的目的地，然后，對(duì)GPS 返回的數(shù)據(jù)進(jìn)行解析，使用高德地圖API[8]查詢(xún)出當(dāng)前位置和目標(biāo)位置，并通過(guò)高德地圖API 完成路徑信息的規(guī)劃，將數(shù)據(jù)發(fā)送至運(yùn)動(dòng)控制部分，最后對(duì)數(shù)據(jù)進(jìn)行處理實(shí)現(xiàn)自主導(dǎo)航。 系統(tǒng)工作框圖如圖2 所示。

圖2 系統(tǒng)工作框圖Fig.2 System workblock diagram

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

2.1 GPS 定位及語(yǔ)音識(shí)別

使用GPS 模塊WTGPS+BD 實(shí)現(xiàn)定位， 提供初始位置， 語(yǔ)音識(shí)別使用百度的AI 平臺(tái)對(duì)實(shí)時(shí)輸入的語(yǔ)音進(jìn)行識(shí)別，提供目的地位置。

2.2 路徑規(guī)劃及數(shù)據(jù)解析

本文介紹的系統(tǒng)在樹(shù)莓派上建立上位機(jī)服務(wù)程序，將當(dāng)前位置、目的地位置的經(jīng)緯度作為輸入?yún)?shù)構(gòu)造出一個(gè)URL，獲取服務(wù)器返回的數(shù)據(jù)對(duì)其進(jìn)行解析，最后按照一定的通信協(xié)議打包發(fā)送。 運(yùn)動(dòng)控制獲取到路徑信息后，以規(guī)定通信協(xié)議進(jìn)行數(shù)據(jù)解析，分別獲取到所有的路徑節(jié)點(diǎn)總數(shù)、單個(gè)路徑節(jié)點(diǎn)數(shù)據(jù)， 在獲取到所有經(jīng)緯度數(shù)據(jù)后開(kāi)啟導(dǎo)航，并實(shí)時(shí)獲取樹(shù)莓派發(fā)送的經(jīng)緯度數(shù)據(jù)，以便實(shí)現(xiàn)在導(dǎo)航時(shí)對(duì)軌跡優(yōu)化的計(jì)算。

3 基于一階多項(xiàng)式曲線(xiàn)優(yōu)化的PID 控制算法

3.1 控制算法

控制算法主要包括兩部分：PID 算法和一階多項(xiàng)式曲線(xiàn)優(yōu)化算法。

PID 是當(dāng)下最為廣泛的控制器，PID 算法具有結(jié)構(gòu)簡(jiǎn)單、原理簡(jiǎn)單、參數(shù)整定的優(yōu)點(diǎn)，并具有很好的穩(wěn)定性和魯棒性[9]。PID 調(diào)節(jié)器主要是對(duì)誤差值的調(diào)節(jié)，誤差值在一定程度上反映了當(dāng)前位置與目標(biāo)位置的一個(gè)相對(duì)距離，通過(guò)對(duì)誤差值的修正，進(jìn)而得到不同輪子的轉(zhuǎn)速，達(dá)到修正，本系統(tǒng)使用的位置式PID，如公式（1）所示：

首先獲取出的誤差值e（k），對(duì)誤差值比例放大，即乘以Kp，再計(jì)算微分量，即當(dāng)前的誤差值減去上一次的誤差值：e（k）-e（k-1），最后再添加積分量，積分量應(yīng)該在系統(tǒng)接近穩(wěn)定時(shí)消除穩(wěn)態(tài)誤差，對(duì)誤差累加積分，消除系統(tǒng)的穩(wěn)態(tài)誤差。

但實(shí)際使用過(guò)程中，會(huì)出現(xiàn)積分項(xiàng)過(guò)大導(dǎo)致系統(tǒng)輸出錯(cuò)誤，所以為防止積分項(xiàng)過(guò)大導(dǎo)致輸出出現(xiàn)過(guò)調(diào)，加入了積分限幅[10]參數(shù)，對(duì)于PID 調(diào)節(jié)器來(lái)說(shuō)，其工作過(guò)程是一個(gè)不斷修正、矯正的環(huán)節(jié)，如果不斷地積分，就很有可能導(dǎo)致積分飽和，輸出的結(jié)果過(guò)大使對(duì)象無(wú)法執(zhí)行，甚至出錯(cuò)，加入限幅可以有效地使系統(tǒng)更加穩(wěn)定。

如圖3 所示，其中PID 輸入量、誤差值可以由一階多項(xiàng)式曲線(xiàn)得出。

圖3 PID 算法框圖Fig.3 PID algorithm block diagram

在控制模塊析出樹(shù)莓派發(fā)送的定位數(shù)據(jù)后，使用一階多項(xiàng)式曲線(xiàn)實(shí)現(xiàn)模型車(chē)在節(jié)點(diǎn)與節(jié)點(diǎn)之間運(yùn)行軌跡的偏差優(yōu)化。 具體采用靜坐標(biāo)系作為參考，模型車(chē)在此坐標(biāo)系下進(jìn)行運(yùn)動(dòng)。

在運(yùn)動(dòng)過(guò)程中，偏差值通過(guò)節(jié)點(diǎn)分段與實(shí)時(shí)位置誤差計(jì)算來(lái)獲取。 假設(shè)路徑Route 中共有n 個(gè)節(jié)點(diǎn)，其中N0為起點(diǎn)，Nn為終點(diǎn)，相鄰兩節(jié)點(diǎn)做路程分段處理，設(shè)段位值P（paragraphs）初始為1，表示介于N0與N1之間，即Pn表示Nn-1Nn段，為最后一段路程，在P=1 時(shí)，當(dāng)?shù)竭_(dá)節(jié)點(diǎn)N1，則P=P+1，進(jìn)入下一段路程，直至P=n 時(shí)，當(dāng)?shù)竭_(dá)節(jié)點(diǎn)Nn表示結(jié)束，并到達(dá)終點(diǎn)。

根據(jù)每段起點(diǎn)與終點(diǎn)的經(jīng)緯度即可求出對(duì)應(yīng)的直線(xiàn)方程，如公式（2）所示：

式中：y=Latitude（緯度）；x=Longitude（經(jīng)度）。

另外還需考慮模型車(chē)在靜坐標(biāo)系下修正方向與運(yùn)動(dòng)方向的關(guān)系，如圖2 所示，用A1～A8八個(gè)點(diǎn)構(gòu)成一個(gè)靜坐標(biāo)系下的環(huán)形軌跡，B 點(diǎn)作為誤差點(diǎn)，其中A1，A2縱坐標(biāo)相同，A3，A4橫坐標(biāo)相同，A5，A6縱坐標(biāo)相同，A7，A8橫坐標(biāo)相同。 軌跡上A1點(diǎn)沿著順時(shí)針?lè)较蛟谲壽E上進(jìn)行運(yùn)動(dòng)， 則依次經(jīng)過(guò)線(xiàn)段A1A2（k=0），A2A3（k<0），A3A4（k=∞），A4A5（k>0），A5A6（k=0），A6A7（k<0），A7A8（k=∞），A8A1（k>0），其中軌跡中對(duì)角位置運(yùn)動(dòng)方向相反，因此在靜坐標(biāo)系下左右修正方向亦相反。 因此需考慮不同斜率下不同運(yùn)動(dòng)方向的偏差計(jì)算與修正。 靜坐標(biāo)系下修正方向與運(yùn)動(dòng)方向關(guān)系如圖4 所示。

圖4 靜坐標(biāo)系下修正方向與運(yùn)動(dòng)方向關(guān)系示意圖Fig.4 Relationship between corrected direction and motion direction in static coordinate system

設(shè)實(shí)時(shí)經(jīng)度為Xnow，緯度為Ynow，終點(diǎn)經(jīng)度為Xfinish，緯度為Yfinish，運(yùn)動(dòng)方向?yàn)閐ir，dir 計(jì)算方法如公式（3）所示：

當(dāng)dir>0 表示正方向運(yùn)動(dòng)，即包括A6A7，A7A8，A8A1，A1A2段；

當(dāng)dir<0 表示負(fù)方向運(yùn)動(dòng)，即包括A2A3，A3A4，A4A5，A5A6段。

當(dāng)運(yùn)動(dòng)軌跡出現(xiàn)偏差，運(yùn)動(dòng)到如圖4 中B1點(diǎn)所示位置，需將其進(jìn)行修正至A2A3線(xiàn)段上，其斜率kA2A3<0，dirA2A3<0，因此在靜坐標(biāo)系下按照其運(yùn)動(dòng)方向應(yīng)當(dāng)向左進(jìn)行修正，并且修正偏差值ErrorB1可通過(guò)公式（4）進(jìn)行計(jì)算：

式中：XB1為B1點(diǎn)經(jīng)度值；XB1′為B1點(diǎn)以緯度作為標(biāo)定，帶入公式（2）計(jì)算得到的線(xiàn)上經(jīng)度值，如公式（5）所示：

將公式（5）代入公式（4）即可得到B1點(diǎn)在線(xiàn)段A2A3上的實(shí)際偏差值ErrorB1<0，采用同樣的方法可得到B2點(diǎn)實(shí)際偏差值ErrorB2>0。

即得到轉(zhuǎn)向修正公式（6）：

相對(duì)于同斜率反方向運(yùn)行的A6A7段，其斜率kA6A7<0，dirA6A7>0，其附近B3偏差點(diǎn)而言需要向右進(jìn)行修正，為滿(mǎn)足公式（6）條件，因此B3點(diǎn)實(shí)際偏差值可由公式（7）計(jì)算可知ErrorB3>0。

同理可知k>0 與k=∞在此靜坐標(biāo)系下方向修正與運(yùn)動(dòng)方向情況與k<0 相同，但在k=∞時(shí)經(jīng)度值處處相等，因此計(jì)算XB′滿(mǎn)足公式（8）：

綜上所述，在k≠0 時(shí)，其誤差計(jì)算方法如公式（9）所示：

式中：XB為k≠0 時(shí)誤差點(diǎn)的經(jīng)度值；XB′為線(xiàn)上修正值，XB′計(jì)算如公式（10）所示：

在k=0時(shí)緯度值處處相等，因此YB′=Yfinish，其誤差計(jì)算方法如公式（11）所示：

式中：YB為k=0 時(shí)誤差點(diǎn)的緯度值。

根據(jù)上述式（9）～式（11）可計(jì)算出圖2 所示的任意一偏差點(diǎn)對(duì)應(yīng)所示線(xiàn)段的偏差值ErrorB， 由公式（6）可知，偏差值ErrorB即反應(yīng)了電機(jī)轉(zhuǎn)數(shù)，因此將偏差值進(jìn)行PID 調(diào)節(jié)后轉(zhuǎn)換成電機(jī)轉(zhuǎn)數(shù)送至電機(jī)控制函數(shù)即可實(shí)現(xiàn)模型轉(zhuǎn)向控制。

3.2 運(yùn)動(dòng)控制算法驗(yàn)證

根據(jù)分析得出的計(jì)算公式計(jì)算出其偏差值與對(duì)應(yīng)直線(xiàn)方程的k，b 進(jìn)行輸出。 并將偏差值送至PID調(diào)節(jié)器進(jìn)行輸出控制模型車(chē)轉(zhuǎn)向進(jìn)行測(cè)試，在實(shí)際測(cè)試過(guò)程中以南京工程學(xué)院（2 號(hào)門(mén)）到南京工程學(xué)院工程實(shí)踐中心4 號(hào)樓為例進(jìn)行自主導(dǎo)航，其部分測(cè)試數(shù)據(jù)如圖5 和圖6 所示。 圖5 為規(guī)劃好的路徑圖，圖6 中陰影為軌跡運(yùn)行點(diǎn)，折線(xiàn)段為路徑，模型能夠通過(guò)GPS 返回的數(shù)據(jù)進(jìn)行誤差判斷并進(jìn)行自主調(diào)節(jié)，因此該運(yùn)動(dòng)控制算法驗(yàn)證通過(guò)。

圖5 軌跡圖Fig.5 Trajectory diagram

圖6 軌跡驗(yàn)證圖Fig.6 Trajectory verification diagram

4 結(jié)語(yǔ)

本文主要介紹了自主導(dǎo)航控制系統(tǒng)的研究和設(shè)計(jì)，主要對(duì)路徑數(shù)據(jù)獲取、數(shù)據(jù)接收與處理進(jìn)行了系統(tǒng)設(shè)計(jì)與介紹。 并對(duì)軌跡優(yōu)化算法進(jìn)行了論證與說(shuō)明，通過(guò)理論分析與公式推導(dǎo)，對(duì)靜態(tài)坐標(biāo)系下一階多項(xiàng)式曲線(xiàn)軌跡優(yōu)化算法的運(yùn)動(dòng)軌跡誤差值進(jìn)行分析，最后通過(guò)模型實(shí)驗(yàn)驗(yàn)證，并得出結(jié)論。