基于GPS定位的自主運動導航系統研究

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

（南京工程學院 創新創業學院，南京 211167）

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

1 系統整體框架

1.1 模塊關系圖

本系統主要由GPS 定位模塊、 語音交互模塊、路徑規劃模塊、運動控制模塊4 部分組成，如圖1所示。

圖1 模塊關系圖Fig.1 Module diagram

GPS 模塊獲取當前位置的經緯度信息，語音模塊識別用戶發出的指令，樹莓派整合數據鏈接高德地圖進行路徑規劃獲取數據，將數據處理后發送至運動控制模塊進行控制。

1.2 系統工作框圖

在系統工作時，首先通過語音識別識別出用戶想要到達的目的地，然后，對GPS 返回的數據進行解析，使用高德地圖API[8]查詢出當前位置和目標位置，并通過高德地圖API 完成路徑信息的規劃，將數據發送至運動控制部分，最后對數據進行處理實現自主導航。 系統工作框圖如圖2 所示。

圖2 系統工作框圖Fig.2 System workblock diagram

2 模塊設計

2.1 GPS 定位及語音識別

使用GPS 模塊WTGPS+BD 實現定位， 提供初始位置， 語音識別使用百度的AI 平臺對實時輸入的語音進行識別，提供目的地位置。

2.2 路徑規劃及數據解析

本文介紹的系統在樹莓派上建立上位機服務程序，將當前位置、目的地位置的經緯度作為輸入參數構造出一個URL，獲取服務器返回的數據對其進行解析，最后按照一定的通信協議打包發送。 運動控制獲取到路徑信息后，以規定通信協議進行數據解析，分別獲取到所有的路徑節點總數、單個路徑節點數據， 在獲取到所有經緯度數據后開啟導航，并實時獲取樹莓派發送的經緯度數據，以便實現在導航時對軌跡優化的計算。

3 基于一階多項式曲線優化的PID 控制算法

3.1 控制算法

控制算法主要包括兩部分：PID 算法和一階多項式曲線優化算法。

PID 是當下最為廣泛的控制器，PID 算法具有結構簡單、原理簡單、參數整定的優點，并具有很好的穩定性和魯棒性[9]。PID 調節器主要是對誤差值的調節，誤差值在一定程度上反映了當前位置與目標位置的一個相對距離，通過對誤差值的修正，進而得到不同輪子的轉速，達到修正，本系統使用的位置式PID，如公式（1）所示：

首先獲取出的誤差值e（k），對誤差值比例放大，即乘以Kp，再計算微分量，即當前的誤差值減去上一次的誤差值：e（k）-e（k-1），最后再添加積分量，積分量應該在系統接近穩定時消除穩態誤差，對誤差累加積分，消除系統的穩態誤差。

但實際使用過程中，會出現積分項過大導致系統輸出錯誤，所以為防止積分項過大導致輸出出現過調，加入了積分限幅[10]參數，對于PID 調節器來說，其工作過程是一個不斷修正、矯正的環節，如果不斷地積分，就很有可能導致積分飽和，輸出的結果過大使對象無法執行，甚至出錯，加入限幅可以有效地使系統更加穩定。

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

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

在控制模塊析出樹莓派發送的定位數據后，使用一階多項式曲線實現模型車在節點與節點之間運行軌跡的偏差優化。 具體采用靜坐標系作為參考，模型車在此坐標系下進行運動。

在運動過程中，偏差值通過節點分段與實時位置誤差計算來獲取。 假設路徑Route 中共有n 個節點，其中N0為起點，Nn為終點，相鄰兩節點做路程分段處理，設段位值P（paragraphs）初始為1，表示介于N0與N1之間，即Pn表示Nn-1Nn段，為最后一段路程，在P=1 時，當到達節點N1，則P=P+1，進入下一段路程，直至P=n 時，當到達節點Nn表示結束，并到達終點。

根據每段起點與終點的經緯度即可求出對應的直線方程，如公式（2）所示：

式中：y=Latitude（緯度）；x=Longitude（經度）。

另外還需考慮模型車在靜坐標系下修正方向與運動方向的關系，如圖2 所示，用A1～A8八個點構成一個靜坐標系下的環形軌跡，B 點作為誤差點，其中A1，A2縱坐標相同，A3，A4橫坐標相同，A5，A6縱坐標相同，A7，A8橫坐標相同。 軌跡上A1點沿著順時針方向在軌跡上進行運動， 則依次經過線段A1A2（k=0），A2A3（k<0），A3A4（k=∞），A4A5（k>0），A5A6（k=0），A6A7（k<0），A7A8（k=∞），A8A1（k>0），其中軌跡中對角位置運動方向相反，因此在靜坐標系下左右修正方向亦相反。 因此需考慮不同斜率下不同運動方向的偏差計算與修正。 靜坐標系下修正方向與運動方向關系如圖4 所示。

圖4 靜坐標系下修正方向與運動方向關系示意圖Fig.4 Relationship between corrected direction and motion direction in static coordinate system

設實時經度為Xnow，緯度為Ynow，終點經度為Xfinish，緯度為Yfinish，運動方向為dir，dir 計算方法如公式（3）所示：

當dir>0 表示正方向運動，即包括A6A7，A7A8，A8A1，A1A2段；

當dir<0 表示負方向運動，即包括A2A3，A3A4，A4A5，A5A6段。

當運動軌跡出現偏差，運動到如圖4 中B1點所示位置，需將其進行修正至A2A3線段上，其斜率kA2A3<0，dirA2A3<0，因此在靜坐標系下按照其運動方向應當向左進行修正，并且修正偏差值ErrorB1可通過公式（4）進行計算：

式中：XB1為B1點經度值；XB1′為B1點以緯度作為標定，帶入公式（2）計算得到的線上經度值，如公式（5）所示：

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

即得到轉向修正公式（6）：

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

同理可知k>0 與k=∞在此靜坐標系下方向修正與運動方向情況與k<0 相同，但在k=∞時經度值處處相等，因此計算XB′滿足公式（8）：

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

式中：XB為k≠0 時誤差點的經度值；XB′為線上修正值，XB′計算如公式（10）所示：

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

式中：YB為k=0 時誤差點的緯度值。

根據上述式（9）～式（11）可計算出圖2 所示的任意一偏差點對應所示線段的偏差值ErrorB， 由公式（6）可知，偏差值ErrorB即反應了電機轉數，因此將偏差值進行PID 調節后轉換成電機轉數送至電機控制函數即可實現模型轉向控制。

3.2 運動控制算法驗證

根據分析得出的計算公式計算出其偏差值與對應直線方程的k，b 進行輸出。 并將偏差值送至PID調節器進行輸出控制模型車轉向進行測試，在實際測試過程中以南京工程學院（2 號門）到南京工程學院工程實踐中心4 號樓為例進行自主導航，其部分測試數據如圖5 和圖6 所示。 圖5 為規劃好的路徑圖，圖6 中陰影為軌跡運行點，折線段為路徑，模型能夠通過GPS 返回的數據進行誤差判斷并進行自主調節，因此該運動控制算法驗證通過。

圖5 軌跡圖Fig.5 Trajectory diagram

圖6 軌跡驗證圖Fig.6 Trajectory verification diagram

4 結語

本文主要介紹了自主導航控制系統的研究和設計，主要對路徑數據獲取、數據接收與處理進行了系統設計與介紹。 并對軌跡優化算法進行了論證與說明，通過理論分析與公式推導，對靜態坐標系下一階多項式曲線軌跡優化算法的運動軌跡誤差值進行分析，最后通過模型實驗驗證，并得出結論。