基于RTK 的增量PID 差速小車循跡實現

朱海波，李毅

（四川大學計算機學院，成都610065）

0 引言

自主循跡是無人駕駛的基礎，也是智能車輛研究的核心技術之一。定位則是第一個要解決的問題。在無人車駕駛系統中普遍采用GNSS 技術，通過一組衛星（至少四顆衛星）的偽距、星歷及用戶鐘差等測量值，為地球表面或近地空間的地點提供三維坐標和速度以及時間信息。但常規的GNSS 由于存在誤差，且無法消除，所以導致精度有限，一般最好在米級。利用RTK技術可以最大程度的減小電離層延遲和對流層延遲帶來的誤差，可以使精度到達厘米級[1]。但RTK 也存在距離的限制，由于基站校正數據的有效作用范圍有限，當與基準站距離過大時，定位精度會下降，所以移動站與基準站的距離在單頻下不能超過10 千米，雙頻不能超過30 千米。

實現車輛自主循跡的核心內容包括如下三個：RTK-BDS 信息采集與存儲；將采集的RTK-BDS 信息形成目標軌跡；參照目標軌跡與當前定位信息，依據增量式PID 控制算法，將實測橫向軌跡與預期軌跡的差作為PID 偏差度量，生成控制命令，實現智能車輛的自主行駛。

1 算法原理

1.1 RTK基本原理

RTK 系統硬件上主要由三部分組成，即基準站、移動站和兩者的通信鏈路。RTK 定位原理[2]是基準站上的BDS 天線和流動站上的BDS 天線同時接收一組衛星信號，兩個BDS 天線將接收到的衛星信號再分別傳遞給基準站BDS 接收機和流動站上的BDS 接收機。由于基準站被安置在某一固定位置，因此基準站的坐標作為固定值已知。根據基準站的坐標和已知衛星的距離，可以反算出衛星的坐標，通過比較解算出差分改正數。同時基準站將解算出的差分改正數通過基準站和移動站的數據鏈路發送給移動站。移動站通過差分改正數和其接收的GPS 數據解算出精確的用戶坐標。之所以選用中國北斗導航系統（BDS）而不選用全球定位系統（GPS）是由于經過實地測量，測試地所處環境下，BDS 可見星數要大于GPS，精度因子（DOP）較小，定位誤差更小。BDS-RTK 由于定位接收器在被建筑遮擋時，導致可見衛星數目大大減小，極易從非固定解狀態，變成浮動解，使得定位出現偏差，所以無法在高大建筑物遮擋的環境下進行。

載波相位差分技術中，基準站接收機的相位為：

公式中c 表示光速，δti為接收機鐘差，δtj為第j顆衛星鐘差，為電離層誤差，為大氣層誤差，δMi為多路徑效應，Vi為GPS 接收機噪聲。在基準站可求出偽距改正數：

用此改正數對用戶站偽距觀測值進行校正，有：

當基準站與用戶站的間距小于30km，可認為

式中Δδρ=c?( δtk-δti)+(δMk-δMi)+(Vk-Vi)。

將載波相位偽距觀測值（2）代入上式，則可得：

上式中令Nj( t0)=( t0)-( t0)為起始整周數之差，在觀測過程中若衛星跟蹤不失鎖，Nj( t0)即為常數，設載波相位測量差值：

式（7）可以表示為：

公式中的λNj( t0)、Xk、Yk、Zk，Δφ 均為常數，僅Δδρ 未知。但兩接收機鐘差之差、噪聲之差及兩站間多路徑之差在相鄰歷元間的變換量均小于厘米級動態定位允許的誤差，所以可將Δδρ 視作常數。

1.2 增量PID基本原理

比例積分微分控制，通過被控對象的輸出來影響PID 控制器的輸出，形成閉環，是一種經典的負反饋控制方法，其特點為算法較為簡單、魯棒性好并且可靠性較高[3]。

比例控制KP能加快系統的動態響應速度，快速地對誤差進行反應，從而減少誤差，但無法消除穩態誤差。KP越大，控制器對于偏差的反應越快，但過大的增益會降低系統相對的穩定性；越小則反應越慢，會花費較多時間進入到穩定狀態。一般而言需要使得KP盡可能的大。

積分控制KI，能夠消除無法通過KP和KD解決的穩態誤差（即系統進入了一個存在誤差的穩態，但并非是預期的穩態）。積分作用將會不斷積累誤差，輸出控制量來消除誤差，直至偏差為零時積分停止，但也存在一個問題，其控制作用較慢。

微分控制KD，與變化率有關，可以減小超調量來克服震蕩，使系統穩定性提高，同時加快響應速度，使系統更快有更好的動態性能。

增量式PID 控制，數字PID 控制算法的一種基本形式，是通過對控制量的增量（本次控制量和上次控制量的差值）進行PID 控制的一種控制算法，是遞推式算法。表示第n 時刻的控制量，表示控制量的增量，e[n]表示n 時刻的誤差。

公式：

通過位置式PID 公式推出[4]：：

1.3 循跡策略

通過RTK-BDS 獲取對應的NEMA-0183 格式的定位數據，將經度與緯度通過高斯投影映射成高斯坐標系下的三維坐標。首先錄入預想軌跡點，通過B 樣條擬合出車輛行駛軌跡。通過計算車輛當前位置距軌跡的距離，作為PID 糾正偏差。通過調節PID 參數使得該偏差盡可能的減小，使車輛實現自主的循跡。

從軌跡起點P0起，對軌跡以K 米的距離取點（P0,…,Pn）。將問題讓車輛擬合一段長且復雜的曲線變成擬合一段短且簡單的曲線。設無人車所在位置為Q。

（1）啟動時，先判斷車輛在哪一段較短軌跡中。首先計算車輛當前位置Q 與P0～Pn中最靠近的點Pi，選定PiPi+1作為當前待擬合的曲線。

（2）過點Q 作PiPi+1連線的垂線，交Pi-1Pi圖形點R，QR 的距離即為PID 需要消除的偏差。通過PID 獲取車輛的輸入量，將輸入量轉換成無人車控制命令，發送到小車。

（3）當Q 經過點Pi+1所在位置切線的垂線時，表示車輛已經來到下一條曲線Pi+1Pi+2。

（4）若未到達終點，則重復步驟（2-4）；若到達終點，則停止。

2 實驗設計

2.1 實驗準備

（1）實驗小車

實驗小車為四輪小車，長為595 毫米，寬為631 毫米，高為217 毫米，前輪作為驅動輪，后輪為萬向輪，作為從動輪。兩個主動輪連接PWM 直流電機。通過增減PWM 電機占空比實現車速控制；通過小車主動輪左右輪差速來進行轉彎控制。通過串口與PC 進行通信，波特率115200bps，數據位為8，停止位1，無校驗?？刂泼罡袷饺绫?。

表1 車輛串口控制命令格式

第0 位控制A 電機，也就是右輪電機速度大小，第1 位控制B 電機，也就是左輪電機速度大小，第0 位與第1 位范圍為0～0xFF。第2 位與第3 位分別表示右輪電機轉向與左輪電機轉向，范圍為0～1，0 表示正向，1 表示負向。

（2）基準站與移動站

將NEO-M8T 作為基準站和基準站的定位信息接受模塊，通過局域網建立基準站與移動站的數據鏈路，解算之后，以數據格式NEMA0183 通過Socket 通信傳到PC。

2.2 實驗流程

首先規劃軌跡，將規劃好的軌跡錄入小車PC 中。將小車置于起點處，小車上的PC 接受RTK-RDB 傳送的定位信息，根據循跡策略進行循跡。

實驗設計為兩組，第一組為循跡直線，將小車移動至偏離預計軌跡2m 位置處，第二組實驗為循跡彎道，兩組實驗觀察實際軌跡與預期軌跡的差距。

2.3 實驗結果

（1）循跡直線

直線循跡結果圖見圖1。

圖1 循跡直線定位結果圖

（2）循跡彎道

彎道循跡結果圖見圖2。

圖2 循跡彎道定位結果圖（起點為右上角）

2.4 結果分析

在實際環境中，通過試湊法調整PID 參數，獲取了效果較好的參數，實現了循跡直線與彎道。在循跡彎道的實驗中，在轉彎過程中存在一定的偏差，分析得KP和KD參數仍然過小，導致無法及時消除誤差。其中原因也與PID 參數沒有有效的調整方法有關，無法得到最優的PID 參數。

3 結語

本文首先介紹了RTK 定位與增量式PID 控制的原理，經過實驗，以RTK-RDB 獲取厘米級精度的定位信息，通過增量式PID 控制實現了車輛的循跡。實驗表明該策略是可行的，但實驗過程中，無法高效地獲取合適的PID 參數。在下一步研究中，將針對PID 參數調整方法進行研究，針對高效的PID 調節方法進行研究。