基于ROS 的PID 閉環控制仿真實驗設計

王慶，黎文娜

（南京師范大學 泰州學院信息工程學院，江蘇泰州，225300）

0 引言

近年來，隨著機器人應用需求越來越大，越來越多的高校開設了機器人的相關課程乃至成立了機器人的專業，機器人技術是一門交叉性的學科，培養掌握機器人技術的應用型本科人才需要更多創新性的教學思維和實驗方法。ROS（Robot Operating System）是一個適用于機器人的開源的操作系統,它提供了操作系統應有的服務[1]，在機器人控制當中，PID 算法憑借著簡單、高效，易實現等特點依然有著很強的生命力，是本科生需要掌握的控制算法。本實驗在ROS 環境下利用turtlesim 仿真器模擬一個根據指令移動到指定地點的過程，對于運行中出現的誤差，使用比例、積分以及微分控制予以補償，使得仿真器中的小海龜到達指定點位，并分析三種控制在仿真狀態下各自的優缺點以及對最終結果的影響。

1 PID 閉環控制原理

PID 控制算法是一種具有幾十年歷史的經典控制算法，在工業控制中已經得到了廣泛的應用，該算法是控制算法中最經典、最簡單、而又最能體現反饋控制思想的算法[2]，在機器人控制領域該算法依然發揮著不可替代的作用，PID 算法的執行流程是非常簡單的，即利用反饋來檢測偏差信號，并通過偏差信號來控制被控量，而控制器本身就是比例、積分、微分三個環節的相加和。其原理如圖1 所示。

圖1 基本PID 控制算法原理圖

根據圖1 我們考慮在某個特定的時刻t，此時輸入量為rin(t)，輸出量為rout(t)，于是偏差就可計算為err(t)=rin(t)-rout(t)。于是PID 的基本控制規律就可以表示為如下公式：

其中kp為比例帶，TI為積分時間，TD為微分時間，比例就是用來對系統的偏差進行反應，所以只要存在偏差，比例就會起作用。積分主要是用來消除穩態誤差，所謂穩態誤差就是指系統穩定后輸入輸出之間依然存在的差值，而積分就是通過偏差的累計來抵消系統的穩態誤差。而微分則是對偏差的變化趨勢做出反應，根據偏差的變化趨勢實現超前調節，提高反應速度。當在計算機上實現PID 算法時，就必須要將其離散化，假設系統采樣周期為T0，在檢查第K個采樣周期，很顯然系統進行第K 次采樣。此時的偏差可以表示為err(k)=rin(k)-rout(k)，那么積分就可以表示為：err(k)+err(k+1)+……，而微分就可以表示為：(err(k)-err(k-1))/T0，于是可以將第k次采樣時，PID 算法的離散形式表示為：

也可以進一步表示為：

2 仿真實驗設計原理

本實驗是基于ROS 環境下的turtlesim 仿真器運行的，ROS 下的通訊機制常用的有兩種話題(Topic)與服務(Service)，由于有些情況下不需要周期性的獲得一些數據，只想在離散的時間點獲得數據，這樣的情況下，topic 這種單方向的頻繁發布消息的通訊模式就顯得不那么適用了，這時，這種一問一答的模式就非常好用了，當需要數據的時候來一個請求就可以了，Service 與 topic 最大的不同是，它是雙向通信，一方發布請求查詢等待結果，一方執行，并反饋結果；因此服務分為客戶端請求，和服務端反饋。Service 是同步通訊機制，當客戶端發送一個請求后，服務端幾乎同時做出響應，等待響應完成后，服務端才反饋結果給客戶端；而在客戶端在等待反饋的過程中一直處于阻塞的狀態，直到接受到反饋后，客戶端才會執行接下來的動作。這樣的一種通訊形式的特點是離散，高效，指向性強，不會出現類似話題的形式，一直發布消息大量占用資源，顯然，本實驗更適合采取Service 通訊機制來完成，Service 通訊圖如圖2 所示。

圖2 Service 通訊圖

3 實驗開發環境及軟件流程

本實驗的系統環境為ubuntu20.04，ROS 的版本為noetic，開發環境為Visual Stdio Code,仿真器為ROS 自帶的turtlesim。

實驗設計的總體思路為：在ROS 的運行機制下，客戶端首先發布點位請求，服務端收到請求后，進入服務端的函數，服務端通過PID 算法在設定頻率下不斷的發布控制小烏龜的角速度和線速度的話題給烏龜節點，烏龜運動，同時烏龜也會同時發布自己的位置的話題，當烏龜動作完成后，服務端給客戶端反饋結果[3]。turtlesim 節點是每個在安裝ROS 包的時候就已經在電腦上的，使用時直接調用就可以了，在這個實驗中我們需要在服務器節點將PID 控制算法實現，在終端直接call 服務即可，從而完成實驗。

■3.1 實驗流程

(1)創建功能包turtle2pose。

(2)創建源文件turtleControl.cpp。

(3)編寫服務端PID 閉環控制算法。

(4)創建并定義 srv 文件。

(5)創建 launch 文件。

(6)CMakeLists 修改。

(7)運行launch 文件，使用rosservice 命令發送目標坐標，通過觀察turtlesim 仿真器及plotjuggler 軟件分析比例、積分、微分控制對實驗結果的影響。

本實驗采用C++語言編程實現控制算法，仿真器頻率設置為50Hz，小烏龜需要根據命令從一個點運動到另一個點，需要兩個運動步驟轉角和前進，轉角控制了移動的方向，前進控制了移動的距離，如圖3 所示，小烏龜初始點為原點，坐標為(0,0)，要控制小烏龜移動到具體的點，就是要解決轉向和前進的問題。

圖3 turtlesim 仿真器

■3.2 轉角

小烏龜當前的坐標和角度可以通過訂閱/turtle1/pose話題獲得相關的信息，當前角度為pose_z，根據命令中發布的坐標req.x、req.y，通過三角函數atan2(req.y,req.x)可得需要轉角的目標值msg_aim.data，如果直接讓小烏龜以固定速度轉角度，顯然速度越大，轉角的誤差越大，不夠穩定，存在誤差，因此需要PID 控制，讓小烏龜以合適的速度轉到目標角度，目標值與當前值之間存在誤差msg_error.data=msg_aim.data-pose_z，將誤差與合適的系數進行運算轉換為合適的角速度輸出用于調節當前角度值pose_z，v.angular.z=kp·msg_error.data+ki·msg_error_i.data+kd·msg_error_d.data，使之無限接近于目標值msg_aim.data。

■3.3 前進

為了使小烏龜前進的距離準確，同樣需要用到PID 算法進行控制，整體的思路與轉角的方法相似，在程序具體編寫的時候相關的變量都可以復用，首先要算出當前點與目標點的距離即誤差msg_error.data=msg_aim_line.data-sqrt(pow(pose_x-pose_x_start,2)+pow(pose_y-pose_y_start,2))，只需要將誤差進行PID 運算后產生的速度的值賦給線速度，用于調節當前小烏龜的位置，v.linear.x=kp·msg_error.data+ki·msg_error_i.data+kd·msg_error_d.data,使之無限接近于目標msg_aim.data。

■3.4 實驗步驟及現象分析

步驟一：對寫好的功能包使用catkin_make 命令進行編譯。

步驟二：分別對kp，ki，kd進行取值，取不同的值，本實驗目標點坐標取（3，3），當誤差小于0.0001 則認為小烏龜到達設定目標，則跳出程序，在終端中輸出所需時間以及service(服務)的反饋，通過plotjuggler 觀察分析誤差與目標的曲線，并觀察仿真器中小烏龜的運動狀態，分析不同的取值對實驗結果的影響。

比例調節：比例調節即p 調節，在程序中首先設置ki與kd為0，kp分別取值0.2、2、20，編譯程序后啟動turtlesim仿真器，利用rosservice 命令發布目標點坐標，觀察轉角目標/aim/data 以及仿真器中小烏龜當前位置/turtle1/pose/theta 兩個話題以及直行目標/aim_line/data 與當前已直行距離/pose_value_now/data 兩個話題的曲線如圖4～圖7 所示。

圖4 不同系數下的p 調節轉角

圖5 不同系數下的p 調節轉角

圖6 不同系數下的p 調節直行

圖7 不同系數下的p 調節直行

現象分析：比例調節在于成比例的調節誤差，通過上圖可以看到，當小烏龜開始向目標角度轉動以及直行時誤差一旦出現立即產生作用減小誤差，不斷的成比例線性的向目標靠攏，在整個曲線趨于穩定時會發現存在著誤差無法消除，這種誤差稱之為穩態誤差，在比例調節中只有誤差存在，才會有速度產生，所以比例調節必然會有誤差存在。穩態誤差在仿真器中的現實意義則是越靠近目標，誤差越小，速度越小，當速度無法使小烏龜運動時則就停了下來，誤差則一直存在，即為穩態誤差。比例系數kp越小，產生的速度越小，小烏龜運動的越慢，到達穩定的時間越長，反之，比例系數kp越大，產生的速度越大，小烏龜運動的越快，到達穩定的時間越短。但kp并不是越大越好，過大會使仿真器中小烏龜快速的晃動或者轉圈，在曲線圖上反應出來的就是較大的超調和振蕩，導致小烏龜不穩定，因此，不能將kp選取過大，應根據實際發布的目標點的坐標選取合適的比例系數kp，使得穩態誤差控制在合適的范圍內，同時又具有較快的響應時間。

積分調節：積分調節即i 調節，在程序中設置kp=0.2，ki=0.1，kd=0，設置積分的區間，當誤差小于0.5 時，積分調節介入，否則積分過大會造成整個系統的超調和振蕩，編譯程序后啟動turtlesim 仿真器，利用rosservice 命令發布目標點坐標，觀察轉角目標/aim/data 以及仿真器中小烏龜當前位置/turtle1/pose/theta 兩個話題以及直行目標/aim_line/data 與當前已直行距離/pose_value_now/data兩個話題的曲線如圖8、圖9 所示。

圖8 i 調節轉角

圖9 i 調節直行

現象分析：積分調節主要用來消除穩態誤差，只要誤差存在，積分環節就會起作用，本實驗中，在50Hz 的頻率下對當前誤差進行不斷的累計，使得小烏龜的速度不斷變化，產生作用來減小誤差，經過一定時間后如圖所示，位置曲線是收斂的，誤差消除。

微分調節：微分調節即d 調節，在程序中首先設置ki與kd為0，kp分別取值0.5、2、20，編譯程序后啟動turtlesim 仿真器，利用rosservice 命令發布目標點坐標，觀察轉角目標/aim/data 以及仿真器中小烏龜當前位置/turtle1/pose/theta 兩個話題以及直行目標/aim_line/data與當前已直行距離/pose_value_now/data 兩個話題的曲線如圖10～圖13 所示。

圖10 不同系數下的d 調節轉角

圖11 不同系數下的d 調節轉角

圖12 不同系數下的d 調節直行

圖13 不同系數下的d 調節直行

現象分析：微分調節能夠反映誤差的變化趨勢，當誤差的變化變得太大之前，引入一個有效的早期修正信號，從而加快到達目標的動作速度，減小調節時間，在積分調節中，雖說可以消除穩態誤差，但到達目標的時間也被拉長并且會產生超調和震蕩，這時候微分調節的介入，在誤差剛出現的瞬間，不僅能夠根據比例積分的控制作用對誤差作出及時反應，還能夠根據誤差的變化趨勢（速度）通過微分調節提前給出較大的控制作用，將誤差消滅在萌芽狀態，微分環節讓小烏龜運動時減小超調，克服振蕩，加快轉動速度，縮短達到目標的時間[4]，結合曲線圖，如表1、表2 所示，在kp與ki完全相同的情況下，隨著kd的變大，到達目標的時間明顯縮短。

表1 kd取不同值時小烏龜轉角到達目標的時間

表2 kd取不同值時小烏龜直行到達目標的時間

4 結語

本實驗主要涉及了ROS 機器人與PID 閉環控制算法兩方面知識，利用ROS 自帶的turtlesim 仿真器結合ROS 節點的service 機制對仿真器中的小烏龜實現PID 閉環控制作用，實現結果表明仿真器中小烏龜的運動軌跡及曲線符合PID 控制器特性。學生通過本實驗不但可以了解ROS 的運行機制，理解最重要的話題與服務的區別并掌握他們的使用方法,而且對PID 閉環控制有了初步的理解，了解比例、積分、微分調節的意義，為后續PID 控制算法的學習打好基礎。