井下履帶式探測機器人及其運動抗擾控制研究

單杰， 關丙火

(國家能源集團 神東煤炭集團公司， 陜西 神木 719315)

0 引言

當前，中國煤礦安全形勢依然嚴峻，煤礦事故死亡人數是世界上其他產煤國家煤礦事故死亡總數的2～3倍[1-3]。井下災害發生后，救援的最大困難在于原有通信癱瘓、工作通道坍塌堵塞，導致無法進行信息和物質交換。由于不能掌握井下災害情況，地面工作人員很難進行決策并精準施救[4-5]。井下探測機器人可提前布置于井下或從地面釋放至井下進行探測作業，通過輪式或履帶式移動底盤驅動，并攜帶各類探測儀器，不僅可實時探測井下現場環境參數并回傳，還可用于應急通信甚至災后輔助救援[6-8]。由于井下尤其是災后路面環境極其復雜，多為非結構路面，所以對機器人的結構設計、爬坡越障性能及動力驅動算法等提出較高要求[9-10]。

在機器人結構設計方面，為提升機器人災后爬坡越障能力，通常采用履帶式移動平臺。履帶式移動底盤具有較好的移動越障能力，并且能夠原地零半徑轉向，更適應井下復雜的非結構路面環境[11-12]。但與輪式底盤相比，履帶式底盤體積和質量更大，導致其移動和轉向時受到較大路面摩擦力作用，故對驅動機構的驅動性能和控制精度提出更高要求。尤其當履帶式移動機器人在井下進行高精度定點探測或救援作業時，對移動轉向機構的驅動性能和控制精度提出很高要求。

在機器人驅動控制方面，永磁同步電動機(Permanent Magnet Synchronous Motor，PMSM)因具有結構簡單、效率高、功率密度大等優勢而被廣泛應用于井下履帶式機器人動力驅動系統[13]。但井下探測機器人作業環境復雜，驅動電動機需要有較寬的轉速調節范圍，同時，因機器人攜帶多種探測或救援裝備且需進行爬坡越障，易導致負載擾動，具有較強的非線性和不確定性。此外，PMSM本身是多變量、強耦合的非線性系統，目前常規的基于誤差的比例積分微分(Proportional Integral Derivative, PID)控制器很難滿足控制需求[14]。

針對上述難題，本文設計了一種四擺臂井下履帶式探測機器人，分析了機器人運動工況和爬坡越障參數。在此基礎上，針對機器人PMSM，速度環采用自抗擾控制器(Active Disturbance Rejection Control，ADRC)，電流環采用比例積分(Proportional Integral，PI)控制器，采用磁場定向控制(Field-Oriented Control，FOC)技術對PMSM進行驅動，從而提升機器人在井下作業時的響應性能和抗干擾性能。

1 井下履帶式探測機器人移動平臺設計

1.1 機器人整機結構

井下履帶式探測機器人主要包括履帶式移動平臺、擺臂機構、驅動機構、探測系統、通信系統等，如圖1所示。

1—履帶式移動平臺；2—擺臂機構；3—驅動機構； 4—探測系統；5—通信系統。圖1 井下履帶式探測機器人樣機Fig.1 Prototype of underground crawler detection robot

履帶式移動平臺為探測機器人的移動載體，內部設置有動力驅動機構、電源和控制器等，外部兩側安裝有懸掛組件，用以帶動機器人移動。為進一步提升機器人攀爬性能，在本體前后兩側設計四擺臂機構進行輔助越障。擺臂機構由內部電動機驅動進行角度調整。探測系統根據現場實際偵查需求搭載不同的傳感組件。通信系統實現機器人與地面指揮站間的信息交互功能。

1.2 擺臂和動力驅動機構設計

機器人移動采用PMSM驅動，左右各設置1組PMSM，通過減速箱進行減速，以提升轉矩。機器人前后擺臂采用外擺臂結構，內部采用齒輪傳動，包括前擺臂、移動組件、后擺臂、動力驅動機構、擺臂驅動機構，如圖2所示。擺臂減速箱將擺臂驅動電動機的動力源降速增矩后傳送至擺臂組件上。通過擺臂驅動電動機轉動，帶動前后擺臂進行角度調整，實現機器人輔助越障爬坡。

1-1—前擺臂驅動輪；1-2—前擺臂從動輪；1-3—前擺臂履帶； 2-1—承重輪；2-2—移動履帶；3-1—后擺臂驅動輪； 3-2—后擺臂從動輪;3-3—后擺臂履帶；4-1—右驅動電動機； 4-2—右驅動減速箱；4-3—左驅動電動機；4-4—左驅動減速箱； 5-1—前擺臂驅動電動機；5-2—前擺臂減速箱； 5-3—后擺臂驅動電動機；5-4—后擺臂減速箱。圖2 機器人擺臂和動力驅動機構Fig.2 Robot swing and movement arm drive mechanism

1.3 機器人越障分析

機器人運動工況一般包括平地行駛和爬坡越障。在井下實施探測救援時，機器人應具備強悍的爬坡越障性能，而影響其爬坡越障性能的重要因素之一是機器人的質心位置分布。井下履帶式探測機器人擺臂及本體質心情況如圖3所示。

圖3 機器人擺臂及本體質心分布Fig.3 Robot swing arm and body centroid distribution

XbObYb為慣性坐標系，X0O0Y0為本體坐標系，X1O1Y1為前擺臂坐標系，X2O2Y2為后擺臂坐標系;CM，Cf，Cr分別為機器人的本體質心、前后擺臂質心。機器人關鍵機械參數見表1。

根據質心變換方程和質心運動學方程[15]，可計算出本文井下履帶式探測機器人最大爬坡傾角為38°，最大越障高度為359 mm。

根據移動底盤機械特性，建立如下典型工況。工況1：平地直行；工況2：直行爬坡，坡度為38°。機器人在移動過程中受內外部阻力作用。機器人兩側履帶接地壓力為

(1)

式中：pmin，pmax分別為最小和最大接地壓力；m為載荷質量；bw為履帶寬度；lw為履帶接地長度；ew為機器人本體質心CM相對于本體中心線的縱向偏心距。

表1 機器人關鍵機械參數Table 1 The key mechanical parameters of the robot

對于機器人履帶系統與驅動輪、從動輪及承重輪等輪系的內部阻力Fin，計算時取摩擦因數μ=0.3。履帶系統與路面的外部阻力為

(2)

式中：n,K分別為路面土壤變形指數和模量；paver為平均接地壓力，paver∈[pmin,pmax]；cw為機器人本體質心CM相對于本體中心線的橫向偏心距;sw為履帶的軌距。

根據上述參數和實際路面環境，考慮動力輸出減速機的減速比及效率問題，上述2種工況下，探測機器人左右PMSM轉矩和轉速見表2。可看出，當機器人爬坡時轉矩需求較平地直行時要高。

表2 機器人2種工況下PMSM轉矩和轉速Table 2 PMSM torque and rotational speed under two working conditions of the robot

2 PMSM抗擾控制方案

對PMSM進行建模分析，設計速度環和電流環控制器；同時，為實現PMSM精準轉速控制，還需對PMSM內部參數進行解耦控制。

2.1 PMSM數學模型

考慮定子電流d軸分量id=0的矢量控制策略，采用三相正弦波電流驅動的隱極子式PMSM，其狀態方程為

(3)

式中：iq為定子電流q軸分量；ω為角速度；Rs為定子電阻；Li為電動機電感;Ld,Lq為電感d,q軸分量,Ld=Lq=Li；ψf為轉子磁鏈；Pn為電動機極對數；J為轉子轉動慣量；Q為阻尼系數;ud，uq分別為定子電壓d，q軸分量；TL為負載轉矩。

2.2 ADRC結構及數學模型

由于探測機器人攜帶大量觀測儀器且在復雜非結構路面環境運動，各類干擾對PMSM的擾動明顯，容易造成機器人軌跡跟蹤和運動精度差。為提高機器人的運動精度和抗干擾能力，本文采用ADRC對PMSM進行控制[16]。

ADRC最先是為解決常規PID控制器的缺陷而提出[17]，包括跟蹤微分器(Tracking Differentiator，TD)、擴張狀態觀測器(Extended State Observer，ESO)及非線性狀態誤差反饋(Nonlinear State Error Feedback，NLSEF)3個部分，如圖4所示。將ADRC內部的不確定擾動和外部干擾定義為廣義總擾動，利用ESO進行擾動觀測，并利用NLSEF進行擾動補償，從而實現對總擾動的抑制。

圖4 ADRC結構Fig.4 ADRC structure

為方便描述ADRC模型，以一階被控對象為例闡述其抗擾原理：

(4)

式中：x，U，y分別為狀態變量、控制輸入、控制輸出；f(x,w)為系統未知擾動；w為擾動狀態變量；B為輸入增益。

ADRC對應的TD，NLSEF和ESO模型分別為[17]

(5)

(6)

(7)

式中：E0，E1和E2為觀測誤差；z11為期望輸出的跟蹤信號；ydesire為期望輸出；z為狀態變量；v為速度因子；fal(·)為非線性函數[16-17]；α0,α1,α2,δ0,δ1,δ2為fal(·)控制參數；z21，z22為ESO的狀態變量，z22/B用于補償被控對象內外總擾動；β1為NLSEF增益；U0(t)為NLSEF的輸出控制信號；t為時間;β01，β02為ESO增益。

2.3 速度環ADRC設計

PMSM轉速控制直接決定探測機器人的空間位置及機器人能否快速精準跟蹤目標軌跡。

將式(3)改寫為ADRC的標準型：

Qω/J-TL/J+iqPnψf/J

(8)

為提高系統響應性能，采用直接誤差函數代替非線性函數fal(·)，重新設計后的NLSEF和ESO模型分別為

(9)

(10)

式中：ka為ADRC增益；Ndesire為期望轉速；E=z21-ω。

可以看到，此時ESO變為標準的線性觀測器，故可進行極點配置，其特征方程穩定的充要條件是β01>0,β02>0。

2.4 電流環PI控制器設計

由于探測機器人采用id=0的控制策略，所以采用PI控制器就能很好地調節d軸電流。由式(3)可知，定子電流id和iq在d，q軸上產生耦合電動勢，此時對id，iq進行解耦，可得解耦后定子電壓d，q軸分量ud0，uq0：

(11)

式中ωe為PMSM電角速度。

(12)

根據電動機的電流環帶寬γ與時間常數τ的關系：τ=min{Ld/Rs,Lq/Rs}，γ=2π/τ，推算出如下關系：kpd=γLd,kid=γRs,kpq=γLq,kiq=γRs。

2.5 FOC原理

采用FOC方法[18-19]實現對轉矩的精準控制。FOC的本質是對電流進行實時采樣并進行電流/電壓坐標變換，實現空間矢量脈寬調制(Space Vector Pulse Width Modulation，SVPWM)。在進行SVPWM時，需判斷電壓空間矢量所在扇區，確定本開關周期所使用的基本電壓空間矢量，具體原理和步驟見文獻[19]。

3 PMSM抗擾控制仿真分析

3.1 Simulink仿真建模

對PMSM進行仿真實驗，將本文PMSM控制方案與常規的雙PI控制方案進行對比，雙PI控制方案即速度環和電流環均采用PI控制器(PI+PI)。速度環采用ADRC、電流環采用PI控制器(ADRC+PI)的Simulink仿真模型架構如圖5所示。

圖5 采用ADRC+PI方案時PMSM Simulink 仿真模型架構Fig.5 Simulink simulation model architecture of PMSM when ADRC+PI scheme is adopted

采用雙PI控制方案的Simulink仿真模型架構如圖6所示。

3.2 平地直行工況仿真

在平地直行工況下，機器人左右兩側的PMSM轉矩為32 N·m，期望轉速Ndesire=1 000 r/min，定子電阻Rs=0.08 Ω，Li=Ld=Lq=6.5×10-2H，電動機極對數Pn=4，轉子轉動慣量J=1.2×10-3kg·m2，轉子磁鏈ψf=0.143 Wb。當電動機進入平穩期后，持續轉矩為4.2 N·m，在t=0.2 s時引入2 N·m干擾量。

圖6 采用PI+PI方案時PMSM Simulink 仿真模型架構Fig.6 Simulink simulation model architecture of PMSM when PI+PI scheme is adopted

平地直行工況下PMSM轉速N響應曲線如圖7所示。在PMSM啟動階段，速度環采用ADRC時基本沒有超調，而采用PI控制器時超調很明顯，最大超調量超過8%；速度環采用ADRC時調節時間僅為0.04 s，而采用PI控制器時調節時間超過0.1 s。在0.2 s加入擾動后，采用PI控制器時PMSM攝動變化明顯，轉速攝動幅度超過20 r/min，而采用ADRC時PMSM轉速非常穩定，抗干擾能力較強。

圖7 平地直行工況下PMSM轉速響應曲線Fig.7 Speed response curves of PMSM in straight driving condition on flat ground

平地直行工況下PMSM轉矩Te響應曲線如圖8所示。采用ADRC+PI方案時，PMSM轉矩輸出穩定且可精準跟蹤期望值或外部擾動量，尤其當遇到外部轉矩突變擾動時，也能快速恢復到給定參考值；但采用PI+PI方案時，PMSM轉矩則存在一定的波動，無法實現對期望轉矩的跟蹤輸出。

平地直行工況下PMSM相電流響應曲線如圖9所示,Ia,Ib,Ic為三相電流。與PI+PI方案相比，采用ADRC+PI方案時，轉速穩定，轉矩輸出平穩，因此PMSM的相電流幅值更穩定，在PMSM啟動過程中，采用ADRC+PI方案時電流對PMSM的沖擊更小，PMSM啟動更平穩；而在突加負載的情況下，采用ADRC+PI方案的PMSM相電流調節時間更短、波動更小。

圖8 平地直行工況下PMSM轉矩響應曲線Fig.8 Torque response curves of PMSM in straight driving condition on flat ground

(a) ADRC+PI

(a) ADRC+PI

3.3 直行爬坡工況仿真

機器人爬坡時，電動機轉矩比在平地行走時高，持續轉矩為6.7 N·m。當電動機進入平穩期后，在t=0.2 s時引入4.7 N·m干擾量。

直行爬坡工況下PMSM轉速響應曲線如圖10所示。在PMSM啟動階段，速度環采用ADRC時產生一定超調，但超調量較小，僅為1.5%，但采用PI控制器時超調量較大，超過11%，且調節時間較長，為采用ADRC時的3倍。在0.2 s加入擾動后，采用PI控制器時攝動變化較明顯，攝動幅度超過40 r/min，而采用ADRC時PMSM轉速相對穩定，轉速攝動幅度不超過10 r/min，抗干擾能力較強。

直行爬坡工況下PMSM轉矩響應曲線如圖11所示。可看出速度環采用ADRC時，PMSM的轉矩輸出穩定且跟蹤精準，且在外部突變擾動狀態下響應速度更快，跟蹤能力更強。

直行爬坡工況下PMSM相電流響應曲線如圖12所示。速度環采用ADRC時，PMSM的相電流逼近正弦波，矢量控制精準，啟動電流更小、更平穩，有利于提高PMSM控制性能并延長其工作壽命。

圖10 直行爬坡工況下PMSM轉速響應曲線Fig.10 Speed response curves of PMSM in straight climbing condition

圖11 直行爬坡工況下PMSM轉矩響應曲線Fig.11 Torque response curves of PMSM in straight climbing condition

(a) ADRC+PI

(b) PI+PI

4 結論

(1) 設計了適用于非結構復雜路面環境的井下履帶式探測機器人，并進行了爬坡越障性能分析，得出了平地直行和直行爬坡2種工況下機器人PMSM的轉矩和轉速。

(2) 對機器人PMSM進行數學建模，設計了速度環ADRC和電流環PI控制器，并采用FOC技術對PMSM進行驅動。

(3) 對ADRC+PI和常規PI+PI控制方案進行仿真和對比分析，得到2種工況下PMSM的轉速、轉矩、相電流響應曲線，結果表明：在2種工況下，采用ADRC+PI方案時機器人PMSM的轉速和轉矩響應控制更精準，具有更小的超調量和更短的調節時間，應對外部突變干擾的能力更強，能有效提升井下履帶式探測機器人的爬坡越障性能和作業穩定性。