基于ROS 的PID 閉環(huán)控制仿真實驗設計

王慶，黎文娜

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

0 引言

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

1 PID 閉環(huán)控制原理

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

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

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

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

也可以進一步表示為：

2 仿真實驗設計原理

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

圖2 Service 通訊圖

3 實驗開發(fā)環(huán)境及軟件流程

本實驗的系統(tǒng)環(huán)境為ubuntu20.04，ROS 的版本為noetic，開發(fā)環(huán)境為Visual Stdio Code,仿真器為ROS 自帶的turtlesim。

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

■3.1 實驗流程

(1)創(chuàng)建功能包turtle2pose。

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

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

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

(5)創(chuàng)建 launch 文件。

(6)CMakeLists 修改。

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

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

圖3 turtlesim 仿真器

■3.2 轉(zhuǎn)角

小烏龜當前的坐標和角度可以通過訂閱/turtle1/pose話題獲得相關的信息，當前角度為pose_z，根據(jù)命令中發(fā)布的坐標req.x、req.y，通過三角函數(shù)atan2(req.y,req.x)可得需要轉(zhuǎn)角的目標值msg_aim.data，如果直接讓小烏龜以固定速度轉(zhuǎn)角度，顯然速度越大，轉(zhuǎn)角的誤差越大，不夠穩(wěn)定，存在誤差，因此需要PID 控制，讓小烏龜以合適的速度轉(zhuǎn)到目標角度，目標值與當前值之間存在誤差msg_error.data=msg_aim.data-pose_z，將誤差與合適的系數(shù)進行運算轉(zhuǎn)換為合適的角速度輸出用于調(diào)節(jié)當前角度值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 算法進行控制，整體的思路與轉(zhuǎn)角的方法相似，在程序具體編寫的時候相關的變量都可以復用，首先要算出當前點與目標點的距離即誤差msg_error.data=msg_aim_line.data-sqrt(pow(pose_x-pose_x_start,2)+pow(pose_y-pose_y_start,2))，只需要將誤差進行PID 運算后產(chǎn)生的速度的值賦給線速度，用于調(diào)節(jié)當前小烏龜?shù)奈恢茫瑅.linear.x=kp·msg_error.data+ki·msg_error_i.data+kd·msg_error_d.data,使之無限接近于目標msg_aim.data。

■3.4 實驗步驟及現(xiàn)象分析

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

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

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

圖4 不同系數(shù)下的p 調(diào)節(jié)轉(zhuǎn)角

圖5 不同系數(shù)下的p 調(diào)節(jié)轉(zhuǎn)角

圖6 不同系數(shù)下的p 調(diào)節(jié)直行

圖7 不同系數(shù)下的p 調(diào)節(jié)直行

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

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

圖8 i 調(diào)節(jié)轉(zhuǎn)角

圖9 i 調(diào)節(jié)直行

現(xiàn)象分析：積分調(diào)節(jié)主要用來消除穩(wěn)態(tài)誤差，只要誤差存在，積分環(huán)節(jié)就會起作用，本實驗中，在50Hz 的頻率下對當前誤差進行不斷的累計，使得小烏龜?shù)乃俣炔粩嘧兓?，產(chǎn)生作用來減小誤差，經(jīng)過一定時間后如圖所示，位置曲線是收斂的，誤差消除。

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

圖10 不同系數(shù)下的d 調(diào)節(jié)轉(zhuǎn)角

圖11 不同系數(shù)下的d 調(diào)節(jié)轉(zhuǎn)角

圖12 不同系數(shù)下的d 調(diào)節(jié)直行

圖13 不同系數(shù)下的d 調(diào)節(jié)直行

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

表1 kd取不同值時小烏龜轉(zhuǎn)角到達目標的時間

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

4 結(jié)語

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