遠(yuǎn)程遙控裝載機(jī)的延時(shí)補(bǔ)償控制研究

朱本龍，沈少恒，李強(qiáng)，黃歡

（1.221111 江蘇省 徐州市 徐州市檢驗(yàn)檢測(cè)中心；2.221116 江蘇省 徐州市 江蘇師范大學(xué) 電氣工程及自動(dòng)化學(xué)院）

0 引言

裝載機(jī)作為最常見的工程機(jī)械之一，被廣泛應(yīng)用于工程建設(shè)、礦石開采等領(lǐng)域[1]。傳統(tǒng)的裝載機(jī)由駕駛員駕駛，在惡劣的工作環(huán)境下，不僅增加駕駛員的操作難度，還可能對(duì)身體健康造成重大影響，甚至危及人身安全。隨著技術(shù)的發(fā)展，工程機(jī)械的智能化、信息化已成為新的發(fā)展方向[2]。通過計(jì)算機(jī)網(wǎng)絡(luò)遠(yuǎn)程控制裝載機(jī)，可以極大提高工作效率，有效地規(guī)避風(fēng)險(xiǎn)，確保工人的安全。然而，在基于計(jì)算機(jī)網(wǎng)絡(luò)的遠(yuǎn)程控制系統(tǒng)中，傳輸延時(shí)是一個(gè)無法回避的問題，會(huì)極大降低系統(tǒng)的穩(wěn)定性。如何盡可能消除延時(shí)帶來的影響，是目前需要研究和解決的問題[3]。

很多學(xué)者針對(duì)工程車輛的遠(yuǎn)程遙控、延時(shí)補(bǔ)償?shù)葐栴}做了相關(guān)研究。劉學(xué)良等[4]設(shè)計(jì)了一種針對(duì)挖掘機(jī)的遠(yuǎn)程控制系統(tǒng)，該系統(tǒng)通過遙控艙實(shí)現(xiàn)對(duì)挖掘機(jī)的遠(yuǎn)程無線控制及對(duì)工作現(xiàn)場的監(jiān)聽監(jiān)視；溫時(shí)豪[5]深入分析了電動(dòng)挖掘機(jī)的參數(shù)與性能，設(shè)計(jì)了基于無線通信的挖掘機(jī)遠(yuǎn)程控制系統(tǒng)，但是，以上系統(tǒng)沒有考慮延時(shí)補(bǔ)償?shù)膯栴}；高達(dá)輝[6]對(duì)CAN總線通信延時(shí)與丟幀補(bǔ)償進(jìn)行了深入研究，提出一種分布式補(bǔ)償方法，并將該方法應(yīng)用到混合動(dòng)力挖掘機(jī)的電動(dòng)回轉(zhuǎn)系統(tǒng)中，該方法比較復(fù)雜，實(shí)現(xiàn)成本較高。本文設(shè)計(jì)了一套針對(duì)裝載機(jī)的延時(shí)補(bǔ)償控制系統(tǒng)，相比以上研究成果，具有結(jié)構(gòu)簡單、成本較低等優(yōu)點(diǎn)。

1 系統(tǒng)總體結(jié)構(gòu)設(shè)計(jì)

本系統(tǒng)在遠(yuǎn)程遙控過程中，通過安裝在車輛上的攝像機(jī)記錄工作環(huán)境的實(shí)時(shí)影像，運(yùn)用無線局域網(wǎng)傳輸實(shí)時(shí)影像數(shù)據(jù)，控制端通過以太網(wǎng)接收數(shù)據(jù)。操作人員可在控制端發(fā)送命令，實(shí)現(xiàn)對(duì)裝載機(jī)的實(shí)時(shí)控制，或者根據(jù)實(shí)際需求對(duì)裝載機(jī)鏟斗末端的位姿軌跡進(jìn)行規(guī)劃，實(shí)現(xiàn)裝載機(jī)自動(dòng)操控。

實(shí)時(shí)視頻數(shù)據(jù)在網(wǎng)絡(luò)上傳輸都有延時(shí)，無論是網(wǎng)絡(luò)在正常狀態(tài)下的常態(tài)延時(shí)，還是由于網(wǎng)絡(luò)擁塞、設(shè)備故障等原因?qū)е碌耐话l(fā)延時(shí)，都會(huì)降低控制系統(tǒng)的穩(wěn)定性。為了解決這一問題，首先需要實(shí)時(shí)、準(zhǔn)確地測(cè)量出延時(shí)時(shí)間，并在控制系統(tǒng)中對(duì)測(cè)得的延時(shí)進(jìn)行補(bǔ)償。因此，擬采用精準(zhǔn)時(shí)間協(xié)議（Precise Time Protocol，PTP）對(duì)系統(tǒng)延時(shí)進(jìn)行精準(zhǔn)計(jì)算，利用Smith 補(bǔ)償器進(jìn)行延時(shí)補(bǔ)償，并通過仿真驗(yàn)證系統(tǒng)的可行性。

2 網(wǎng)絡(luò)延時(shí)測(cè)量

2.1 PTP 簡介

PTP 是根據(jù)IEEE1588v2 標(biāo)準(zhǔn)提出的高精度時(shí)間同步協(xié)議，可以精確計(jì)算網(wǎng)絡(luò)延時(shí)并實(shí)現(xiàn)10 ns級(jí)精度的時(shí)間同步[7]。PTP 同步原理如圖1 所示。

圖1 PTP 同步原理Fig.1 Synchronization principle of PTP

圖1 中包含4 種報(bào)文，同步報(bào)文（Sync），跟隨報(bào)文（Follow_up），延遲請(qǐng)求報(bào)文（Delay_req），延期請(qǐng)求響應(yīng)報(bào)文（Delay_resp）[8]。同步機(jī)制下報(bào)文的收發(fā)流程為：（1）從主時(shí)鐘發(fā)出Sync 報(bào)文，同時(shí)記錄下Sync 報(bào)文離開主時(shí)鐘的精確發(fā)送時(shí)間t1。Sync 報(bào)文可以攜帶發(fā)送時(shí)間信息，也可以不攜帶，因?yàn)镾ync 報(bào)文攜帶的發(fā)送時(shí)間不是精確的；（2）Follow_up 報(bào)文將精確發(fā)送時(shí)間t1封裝發(fā)送給從時(shí)鐘，與Sync 報(bào)文攜帶的發(fā)送時(shí)間相比，F(xiàn)ollow_up 攜帶的發(fā)送時(shí)間是精確的；（3）從時(shí)鐘記錄下Sync報(bào)文到達(dá)從時(shí)鐘的精確時(shí)間t2；（4）在從時(shí)鐘發(fā)出Delay_req 報(bào)文的同時(shí)記錄下精確發(fā)送時(shí)間t3；（5）主時(shí)鐘記錄下Delay_req 報(bào)文到達(dá)主時(shí)鐘的精確時(shí)間t4；（6）主時(shí)鐘發(fā)出攜帶精準(zhǔn)時(shí)間戳信息t4 的Delay_resp 報(bào)文給從時(shí)鐘。

2.2 延時(shí)的計(jì)算

在系統(tǒng)中指定一個(gè)主時(shí)鐘，用offset表示主從時(shí)鐘之間的偏差值，用delay表示網(wǎng)絡(luò)延時(shí)，即報(bào)文在傳輸過程中的延時(shí)，表達(dá)式分別為

式中：t1，t2，t3，t4——上一節(jié)提到的時(shí)間戳信息。

3 Smith 延時(shí)補(bǔ)償控制

根據(jù)上節(jié)測(cè)量的延時(shí)時(shí)間，本文選用PID 控制器進(jìn)行反饋控制，使等效二連桿的末端跟蹤給定的期望軌跡。采用Smith 補(bǔ)償器進(jìn)行時(shí)間補(bǔ)償，實(shí)現(xiàn)系統(tǒng)的穩(wěn)定。帶純滯后環(huán)節(jié)的控制系統(tǒng)如圖2 所示。

圖2 帶純滯后環(huán)節(jié)的控制系統(tǒng)Fig.2 Control system with pure hysteresis

圖2 中：R（s），Y（s）——控制系統(tǒng)的輸入與輸出；D（s），Gp（s）——控制器與被控對(duì)象的傳遞函數(shù)；e-τs——時(shí)滯因子，表示被控對(duì)象的純滯后環(huán)節(jié)。

該控制系統(tǒng)的傳遞函數(shù)為

由式（2）可以得到系統(tǒng)的特征方程

其中的延時(shí)環(huán)節(jié)會(huì)影響系統(tǒng)的穩(wěn)定性，進(jìn)而影響整個(gè)控制系統(tǒng)的性能。因此，為了提高系統(tǒng)的穩(wěn)定性，應(yīng)盡可能消除純滯后帶來的不利影響。

Smith 補(bǔ)償控制是一種專門針對(duì)純滯后系統(tǒng)的預(yù)估補(bǔ)償控制方案，其基本思路：通過在純滯后控制系統(tǒng)中引入的一個(gè)預(yù)估補(bǔ)償環(huán)節(jié)，消除閉環(huán)特征方程中的純滯后項(xiàng)，從而提高控制質(zhì)量[9]。Smith預(yù)估補(bǔ)償系統(tǒng)如圖3 所示，并聯(lián)在D（s）上的就是Smith 預(yù)估器。

圖3 Smith 預(yù)估器控制系統(tǒng)結(jié)構(gòu)圖Fig.3 Block diagram of Smith predictor control system

圖3 中虛線方框中的部分作為控制器，其傳遞函數(shù)為

經(jīng)Smith 預(yù)估器補(bǔ)償后系統(tǒng)的閉環(huán)傳遞函數(shù)為

由式（5）可知，補(bǔ)償后系統(tǒng)的閉環(huán)特征方程中不再含有純滯后項(xiàng)，系統(tǒng)性能可以得到顯著提升[9]。

4 裝載機(jī)的建模

選用HT-25J 多功能裝載機(jī)作為研究對(duì)象，將裝載機(jī)的鏟斗和動(dòng)臂等效成一個(gè)二連桿機(jī)械臂[10]。本文在MATLAB 環(huán)境中對(duì)等效的機(jī)械臂模型進(jìn)行運(yùn)動(dòng)學(xué)建模與動(dòng)力學(xué)建模。

4.1 D-H 參數(shù)法建模

運(yùn)動(dòng)學(xué)建模是研究機(jī)械臂控制方法和軌跡規(guī)劃的重要環(huán)節(jié)[11]。一般采用D-H 參數(shù)法進(jìn)行運(yùn)動(dòng)學(xué)建模，即在等效二連桿的每個(gè)關(guān)節(jié)上建立一個(gè)空間坐標(biāo)系，通過齊次坐標(biāo)變換建立2 個(gè)不同空間坐標(biāo)系的變換關(guān)系[12]。建立的二連桿的D-H 模型如圖4 所示。圖4 中，設(shè)固定基座為連桿0，其與連桿1 的連接處為關(guān)節(jié)1；連桿1 與連桿2 的連接處為關(guān)節(jié)2；連桿2 的末端為關(guān)節(jié)3。在關(guān)節(jié)1 處建立基坐標(biāo)系和1 號(hào)坐標(biāo)系；在關(guān)節(jié)2 處建立2 號(hào)坐標(biāo)系；在關(guān)節(jié)3 處建立3 號(hào)坐標(biāo)系[13]。根據(jù)建立的機(jī)器人連桿坐標(biāo)系，可以獲得二連桿的D-H 參數(shù)表，如表1 所示。

表1 D-H 模型參數(shù)Tab.1 D-H model parameters

圖4 二連桿機(jī)械臂的D-H 模型Fig.4 D-H Model of two-link arm

表1 中：θ1——關(guān)節(jié)1 轉(zhuǎn)角；θ2——關(guān)節(jié)2 轉(zhuǎn)角；L1——桿1 的長度；L2——桿2 的長度。

通過正運(yùn)動(dòng)學(xué)解算，求得二連桿的末端坐標(biāo)為

與正運(yùn)動(dòng)學(xué)相反，逆運(yùn)動(dòng)學(xué)是根據(jù)已知末端位姿求關(guān)節(jié)的變量。圖4 中，由二連桿的末端點(diǎn)向y軸做垂線，并與基坐標(biāo)系的原點(diǎn)相連，構(gòu)成一個(gè)直角三角形，其邊長分別為X、Y、L0，設(shè)該直角三角形左上角為θ0，則

其中，末端點(diǎn)（x，y）的取值分別為X和-Y。根據(jù)式（6）可得

將式（8）左右兩邊同時(shí)平方后相加得

聯(lián)立式（7）、式（9）得

最終求得

同理可求出θ1+θ2，進(jìn)而求出θ2：

4.2 動(dòng)力學(xué)建模

利用拉格朗日法[14]，對(duì)等效二連桿進(jìn)行動(dòng)力學(xué)建模，建立輸入力矩與關(guān)節(jié)角度之間的聯(lián)系，其動(dòng)力學(xué)方程為

5 系統(tǒng)仿真結(jié)果

首先在OPNET 上進(jìn)行網(wǎng)絡(luò)模擬仿真，得到具體的延時(shí)時(shí)間，然后通過Simulink 進(jìn)行控制系統(tǒng)仿真，具體過程如下：

（1）網(wǎng)絡(luò)環(huán)境仿真

在OPNET 上的網(wǎng)絡(luò)仿真分為3 個(gè)階段，第1階段為網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)設(shè)計(jì)，OPNET 網(wǎng)絡(luò)域設(shè)置攝像頭視覺節(jié)點(diǎn)，通過無線局域網(wǎng)將這些節(jié)點(diǎn)連接到服務(wù)器，服務(wù)器再通過有限局域網(wǎng)傳到客戶端；第2 階段為通訊協(xié)議設(shè)置，攝像頭捕捉的視頻采用MPEG-4 標(biāo)準(zhǔn)編碼，應(yīng)用層采用FTP 協(xié)議，傳輸層采用UDP 協(xié)議，網(wǎng)絡(luò)層采用IP 協(xié)議，無線局域網(wǎng)采用的是IEEE802.11 協(xié)議。同時(shí)，應(yīng)用PTP 協(xié)議使這層網(wǎng)絡(luò)的所有設(shè)備都與該時(shí)鐘保持同步；第3 階段為模擬仿真，在已經(jīng)加入PTP 協(xié)議的無線局域網(wǎng)中模擬網(wǎng)絡(luò)擁塞，通過帶寬配置和設(shè)置多個(gè)節(jié)點(diǎn)數(shù)增加網(wǎng)絡(luò)負(fù)載。通過一系列實(shí)驗(yàn)，在保證沒有數(shù)據(jù)丟失的情況下帶寬配置為1 Mbps，設(shè)置10 個(gè)移動(dòng)站時(shí)測(cè)得的延時(shí)約10 ms。

（2）控制系統(tǒng)的仿真

通過Simulink 建立PID 控制器模塊、動(dòng)力系統(tǒng)仿真模塊以及延時(shí)補(bǔ)償模塊，并與上文建立的等效二連桿模型相結(jié)合，共同構(gòu)成完整的仿真系統(tǒng)，系統(tǒng)的基本結(jié)構(gòu)如圖5 所示。

圖5 控制系統(tǒng)的基本結(jié)構(gòu)Fig.5 Structural diagram of control system

仿真中給定的二連桿末端路徑是半徑為0.5 m的圓弧，將軌跡限制在一個(gè)平面內(nèi)，因此末端位置的橫坐標(biāo)和縱坐標(biāo)均按余弦規(guī)律變化。將逆運(yùn)動(dòng)學(xué)解算后得到的關(guān)節(jié)角作為給定輸入到PID 控制器模塊中。動(dòng)力學(xué)仿真模塊是一個(gè)非線性系統(tǒng)模型，而本文的Smith 預(yù)估器是針對(duì)線性模型的，因此將Smith 預(yù)估器中的系統(tǒng)模型近似成慣性環(huán)節(jié)和比例環(huán)節(jié)的串聯(lián)，延時(shí)時(shí)間設(shè)為10 ms。通過手動(dòng)調(diào)節(jié)Smith 預(yù)估器的參數(shù)，使系統(tǒng)達(dá)到穩(wěn)定，最終在預(yù)估器中慣性時(shí)間常數(shù)為0.04 s，比例放大倍數(shù)為0.001，仿真結(jié)果如圖6 所示。

圖6 運(yùn)行結(jié)果對(duì)比Fig.6 Comparison of control systems

本次仿真僅顯示了機(jī)械臂末端在橫坐標(biāo)上的取值隨時(shí)間的變化情況，圖6（a）是不加Smith 預(yù)估器時(shí)的運(yùn)行結(jié)果，可以看出網(wǎng)絡(luò)延時(shí)使得系統(tǒng)不穩(wěn)定；圖6（b）是加入Smith 預(yù)估器后的結(jié)果，經(jīng)過大約100 s 的調(diào)整時(shí)間，系統(tǒng)保持在一個(gè)較為平穩(wěn)的狀態(tài)，此時(shí)期望值與實(shí)際值基本重合，Smith預(yù)估器取得了良好的效果。

6 結(jié)語

本文設(shè)計(jì)了裝載機(jī)的遠(yuǎn)程控制及延時(shí)補(bǔ)償系統(tǒng)，通過仿真結(jié)果可以看出，Smith 預(yù)估補(bǔ)償取得了良好效果。該系統(tǒng)具有結(jié)構(gòu)簡單且成本低廉的優(yōu)點(diǎn)，在實(shí)際工作中可以極大地降低操作難度，節(jié)約成本。該方法存在的主要問題是補(bǔ)償器的參數(shù)需要根據(jù)不同的控制對(duì)象手動(dòng)調(diào)節(jié)。此外，在實(shí)際系統(tǒng)中的控制效果需要進(jìn)一步驗(yàn)證。