地下鏟運機自主行駛與避障控制方法研究

石 峰，顧洪樞，戰(zhàn) 凱，李建國，馮孝華，郭 鑫，李恒通，陳 維，石金鵬

（1.北京礦冶研究總院，北京100160；2.德納管理（上海）有限公司，上海200443；3.鞍鋼集團(tuán)公司，遼寧鞍山114007）

地下鏟運機自主行駛與避障控制方法研究

石 峰1，顧洪樞1，戰(zhàn) 凱1，李建國1，馮孝華1，郭 鑫1，李恒通1，陳 維2，石金鵬3

（1.北京礦冶研究總院，北京100160；2.德納管理（上海）有限公司，上海200443；3.鞍鋼集團(tuán)公司，遼寧鞍山114007）

由測量數(shù)據(jù)或軌跡推算模型，計算鏟運機橫向位置偏差、航向角偏差及航向角偏差變化率，三偏差通過非線性融合規(guī)則，形成融合偏差反饋信號并作為PID校正控制的輸入量，設(shè)置橫向位置偏差及航向角偏差雙門限值來改進(jìn)PID算法。該自主行駛控制方法在軌跡偏差及行駛車速大范圍變化時，仍能保持優(yōu)良的穩(wěn)定性和快速性。

地下鏟運機；自主行駛；避障；控制算法

隨著地下礦開采深度的增加，采礦條件越來越惡劣，對人的安全威脅也越來越大，遙控采礦、自動化礦山開采技術(shù)應(yīng)運而生［1－3］。地下鏟運機是地下無軌采礦的關(guān)鍵設(shè)備，可以實現(xiàn)地下鏟運機在井下巷道內(nèi)無人操縱、自主行駛，使操作工人遠(yuǎn)離井下惡劣、危險的工作環(huán)境，保護(hù)鏟運機司機人身安全，提高采礦作業(yè)效率，降低采礦成本。

由于井下巷道斷面不規(guī)則，巷道壁凹凸不平，使得沿墻行走導(dǎo)航方式受到一定程度的限制。另外，井下巷道內(nèi)經(jīng)常會沿巷道壁放置通風(fēng)機、電氣開關(guān)柜等設(shè)施，形成局部障礙，使得地下鏟運機并不總是等距離地沿墻行走，避障運動控制是地下自主鏟運機一項必不可少的功能。因此，在地下巷道環(huán)境下，有必要事先規(guī)劃出目標(biāo)路徑，并采用相對定位與絕對定位相結(jié)合的方法，來確定鏟運機相對于目標(biāo)路徑的偏差，通過自主避障行駛控制器的控制作用，使地下鏟運機自主跟蹤目標(biāo)路徑并實現(xiàn)避障［4］。

1 地下鏟運機目標(biāo)路徑及跟蹤軌跡參數(shù)關(guān)系

巷道內(nèi)的地下鏟運機主目標(biāo)路徑、局部避障目標(biāo)路徑、跟蹤軌跡及相關(guān)參數(shù)說明示意圖見圖1。

以巷道路面為坐標(biāo)平面建立x－y坐標(biāo)系，選取鏟運機前方驅(qū)動橋的中點為鏟運機的定位參考點，以該點在x－y坐標(biāo)平面上的投影點P代表鏟運機的位置。以P點的速度方向（軌跡切線方向）作為車輛的運動方向，其與x軸正方向所成的夾角定義為鏟運機的航向角φg，取從x軸逆時針旋轉(zhuǎn)形成的航向角為正，反之為負(fù)。P點的運動軌跡即為鏟運機的跟蹤軌跡，見圖中曲線C。在巷道內(nèi)預(yù)先規(guī)劃好的P點運動目標(biāo)路徑作為鏟運機的主目標(biāo)路徑，見圖中曲線A。為了避障，在巷道內(nèi)局部區(qū)域規(guī)劃出與主目標(biāo)路徑橫向等距偏移的P點運動目標(biāo)路徑為局部避障目標(biāo)路徑，見圖中曲線B，其中，橫向偏離位移用δb0表示，取局部避障目標(biāo)路徑在主目標(biāo)路徑曲線左側(cè)的橫向偏離位移為正，反之為負(fù)。P點在目標(biāo)路徑上的垂直投影點Pm的速度方向與x軸正方向所成的夾角，定義為目標(biāo)路徑的航向角φm。由于局部避障目標(biāo)路徑上每一點是等距且平行于主目標(biāo)路徑上的對應(yīng)點，所以P點在主目標(biāo)路徑上的垂直投影點Pm的速度方向與P點在局部避障目標(biāo)路徑上的垂直投影點Pmb的速度方向是相同的，即主目標(biāo)路徑和局部避障目標(biāo)路徑的航向角均為φm。主目標(biāo)路徑或局部避障目標(biāo)路徑的航向角φm與鏟運機跟蹤軌跡航向角φg的差值，定義為航向角偏差β，β＝φm－φg，該參數(shù)反映了自主鏟運機運動方向與目標(biāo)路徑預(yù)定的行駛方向的偏差。鏟運機跟蹤軌跡上P點與主目標(biāo)路徑上Pm點的距離定義為鏟運機的橫向位置偏差δ。取P點在目標(biāo)路徑曲線右側(cè)的橫向位置偏差為正，反之為負(fù)，該參數(shù)可反映出鏟運機相對于主目標(biāo)路徑的橫向定位情況。鏟運機跟蹤軌跡上P點與局部避障目標(biāo)路徑上Pmb點的距離定義為鏟運機的橫向位置偏差δb。取P點在局部避障目標(biāo)路徑曲線右側(cè)的橫向位置偏差為正，反之為負(fù)。該參數(shù)可反映出鏟運機相對于局部避障目標(biāo)路徑的橫向定位情況，并且有δb＝δ＋δb0。為了計算方便，地下鏟運機定位P點位置坐標(biāo)由對應(yīng)的主目標(biāo)路徑Pm點的曲線弧長sm和鏟運機橫向位置偏差δ來表示，即P（sm，δ）。Kg為跟蹤軌跡在P點的曲率，當(dāng)曲率中心Og在車輛軌跡曲線前進(jìn)方向的左側(cè)時定義為正曲率，曲率半徑Rg為正，反之為負(fù)。Km為主目標(biāo)路徑在Pm點的曲率，Rm為其曲率半徑，Om為曲率中心。Kmb為局部避障目標(biāo)路徑在Pmb點的曲率。鏟運機P點行駛車速vg與軌跡曲線C相切，在實際自主行駛控制過程中，車速vg由安裝在車輛底盤傳動系統(tǒng)的車速傳感器或里程計測量得到。主目標(biāo)路徑上Pm點投影車速為vm，投影車速vm與主目標(biāo)路徑曲線A相切，局部避障目標(biāo)路徑上Pmb點投影車速為vmb，投影車速vmb與局部避障目標(biāo)路徑曲線B相切。取鏟運機運動方向為主目標(biāo)路徑曲線弧長sm的正方向，鏟運機沿局部目標(biāo)路徑運動的曲線弧長為smb。α為鏟運機轉(zhuǎn)向角，即鏟運機前后車體的相對轉(zhuǎn)動角，由安裝在鏟運機前后車體鉸接處的轉(zhuǎn)角傳感器測量得到。使地下鏟運機反時針轉(zhuǎn)彎的轉(zhuǎn)向角α為正，反之為負(fù)。l1為鏟運機前方驅(qū)動橋的中點到鉸接中心線的距離，l2為鏟運機后方驅(qū)動橋的中點到鉸接中心線的距離。

圖1 目標(biāo)路徑、跟蹤軌跡及參數(shù)定義Fig.1 Target path，tracking trajectory and the definition of parameters

經(jīng)過公式推導(dǎo)，可得出以下反映地下鏟運機目標(biāo)路徑及跟蹤軌跡參數(shù)關(guān)系的方程組：

2 三偏差融合自主行駛控制特性分析

2.1 三偏差融合反饋控制

將地下鏟運機自主行駛控制器設(shè)計成三偏差融合反饋結(jié)合PID校正的行駛控制器，其中三偏差融合反饋e的計算公式見式（2）［5］。

式中：δb—包括避障工況的橫向位置偏差，β—航向角偏差為航向角偏差變化率；K1、K2和K3分別為橫向位置偏差、航向角偏差和航向角偏差變化率的反饋系數(shù)，K1、K2和K3的選取范圍均為0～1，且K1＋K2＋K3＝1。較佳的K1、K2和K3系數(shù)值分別為0.2≤K1≤0.3、0.3≤K2≤0.6、0.2≤K3≤0.4。

三偏差信息融合反饋形成三種控制作用的相互制約，避免了單一反饋控制的局限性，使得自主行駛控制具有預(yù)測性和前瞻性，通過改變K1、K2和K3的相對大小，可以改變?nèi)椘钤诜答佒械臋?quán)重，方便地調(diào)節(jié)自主行駛控制系統(tǒng)總的響應(yīng)特性。除了橫向位置偏差δb、航向角偏差β，由公式可知，航向角偏差變化率包含有車速vg、目標(biāo)路徑曲率Km和轉(zhuǎn)向角α，可以將車速vg、目標(biāo)路徑曲率Km和轉(zhuǎn)向角α的影響適當(dāng)反映進(jìn)來，所以該三偏差信息融合自主行駛控制具有較好的控制性能。

2.2 橫向位置偏差限幅控制

對于自主行駛控制系統(tǒng)，在較小的橫向位置偏差δb（如δb＜1m）時，即橫向位置偏差δb在小范圍變化時，控制的快速穩(wěn)定均衡關(guān)系比較容易實現(xiàn)。但是，當(dāng)有較大的橫向位置偏差δb（如δb＞1m）存在時，由上式計算出的三偏差融合反饋e的數(shù)值較大，這時橫向位置偏差δb的作用效果過強，而航向角偏差β和航向角偏差變化率的作用相對較弱，導(dǎo)致自主行駛控制系統(tǒng)超調(diào)加大、振蕩加劇，破壞了三偏差之間的均衡作用，甚至使控制系統(tǒng)失去穩(wěn)定。另外，如果以此時較大的橫向位置偏差δb來調(diào)整自主行駛控制系統(tǒng)參數(shù)K1、K2和K3，使其有較好的控制效果，那么當(dāng)存在較小的橫向位置偏差δb時，自主行駛控制系統(tǒng)的響應(yīng)就會較慢，控制效果也會變差。即當(dāng)橫向位置偏差δb在較大范圍變化時，自主行駛控制系統(tǒng)要么失去快速性，要么失去穩(wěn)定性，兩者很難兼顧。

因此，為了使導(dǎo)航控制效果在橫向位置偏差δb較大范圍變化時，仍有較好的控制效果，對橫向位置偏差δb進(jìn)行限幅控制，限幅值εδ1的取值范圍是0＜εδ1≤1（m）。當(dāng)－εδ1≤δb（k）≤εδ1時，δb（k）＝δb（k）。當(dāng)δb（k）＞εδ1時，δb（k）＝εδ1。當(dāng)δb（k）＜－εδ1時，δb（k）＝－εδ1。通過調(diào)整限幅值εδ1的大小來慢速釋放較大橫向位置偏差δb對導(dǎo)航控制器的過大作用量，保持導(dǎo)航控制系統(tǒng)三偏差之間的均衡作用，實現(xiàn)導(dǎo)航控制系統(tǒng)的快速性與穩(wěn)定性的統(tǒng)一，該方法簡單可靠、調(diào)整方便，對橫向位置偏差δb較佳的限幅范圍是0.4≤εδ1≤0.8（m）。

橫向位置偏差有限幅與無限幅時的跟蹤曲線比較見圖2。此時，橫向位置偏差反饋系數(shù)K1＝0.2，航向角偏差反饋系數(shù)K2＝0.55，航向角變化率偏差反饋系數(shù)K3＝0.25，橫向位置偏差δb限幅值εδ1＝0.6m，橫向偏差初始值δ0＝1m，δ0＝3 m，δ0＝5m，航向角偏差初始值β0＝0°，轉(zhuǎn)向角初始值α0＝0°，車速vg＝8km/h。當(dāng)橫向位置偏差無限幅作用時，當(dāng)δb＞1m時，響應(yīng)超調(diào)加劇，甚至失去穩(wěn)定性；當(dāng)橫向位置偏差有限幅作用時，導(dǎo)航控制系統(tǒng)穩(wěn)定性保持良好，無超調(diào)和失穩(wěn)，跟蹤效果明顯優(yōu)于橫向位置偏差無限幅作用情況。

2.3 雙門限改進(jìn)型PID控制

PID校正控制是工程上最常用的控制方案，PID控制器是最簡單但許多時候仍是最好的控制器［6－7］。但是，常規(guī)的PID校正控制也存在不足之處，如比例控制不能消除穩(wěn)態(tài)誤差，積分作用太強會使系統(tǒng)超調(diào)加大，而積分作用太弱又會使系統(tǒng)消除穩(wěn)態(tài)誤差的作用時間延長，降低系統(tǒng)控制精度。

對PID校正控制進(jìn)行改進(jìn)設(shè)計，分別設(shè)置橫向位置偏差δb門限值εδ2和航向角偏差β門限值εβ，用來檢測鏟運機跟蹤軌跡是否已經(jīng)接近目標(biāo)路徑。當(dāng)鏟運機跟蹤軌跡的橫向位置偏差δb的絕對值大于門限值εδ2時，說明此時鏟運機跟蹤軌跡的橫向位置偏差還較大，這時還不必考慮消除穩(wěn)態(tài)跟蹤誤差，故不必應(yīng)用積分控制，控制系統(tǒng)僅需要根據(jù)三偏差融合反饋e的情況結(jié)合PD校正進(jìn)行控制，從而避免了積分飽和現(xiàn)象，避免了產(chǎn)生過大的超調(diào)。當(dāng)鏟運機跟蹤軌跡的橫向位置偏差δb的絕對值小于門限值εδ2時，這時由于橫向位置偏差δb的數(shù)值較小，產(chǎn)生的控制作用比較弱，加上系統(tǒng)死區(qū)的存在，會使控制系統(tǒng)消除橫向位置偏差δb的能力減弱，調(diào)整時間延長，并產(chǎn)生較大穩(wěn)態(tài)誤差。為了避免這種情況，此時加入積分控制作用，通過積累較小控制作用形成較大控制作用，加大了系統(tǒng)消除較小誤差的能力，減小調(diào)整時間，消除存在的穩(wěn)態(tài)誤差。考慮到在鏟運機橫向位置偏差δb較小的同時，可能存在航向角偏差β數(shù)值較大的情況，如果這時加入積分作用，會使系統(tǒng)超調(diào)加大，振蕩加劇。所以，在設(shè)置橫向位置偏差δb門限值εδ2的同時，設(shè)置航向角偏差β門限值εβ。僅當(dāng)雙門限要求同時滿足時，即且才進(jìn)行融合偏差反饋信號e的PID控制，否則僅進(jìn)行PD控制，以保證系統(tǒng)的控制效果，這樣可避免產(chǎn)生過大的超調(diào)，又使系統(tǒng)有較快的響應(yīng)和較小的穩(wěn)態(tài)誤差。其中，εδ2的取值范圍為0＜εδ2≤0.3（m），較佳的門限值εδ2的取值范圍是0.1＜εδ2≤0.2（m）；εβ的取值范圍為0＜εβ≤0.3（rad），較佳的門限值εβ的取值范圍是0.03＜εβ≤0.2（rad）。

圖2 橫向位置偏差有限幅與無限幅時的跟蹤曲線比較圖Fig.2 Tracking curves comparison between lateral position deviation amplitude limited and not limited

通過橫向位置偏差及航向角偏差雙門限控制，融合偏差反饋信號e獲得較理想PID校正控制效果。雙門限積分控制與常規(guī)積分控制跟蹤曲線比較見圖3，此時橫向位置偏差反饋系數(shù)K1＝0.2，航向角偏差反饋系數(shù)K2＝0.55，航向角變化率偏差反饋系數(shù)K3＝0.25，橫向位置偏差δb的限幅值εδ1＝0.6m，PID的比例系數(shù)Kp＝30，PID的微分系數(shù)Kd＝0，PID的雙門限時積分系數(shù)Ki＝18，常規(guī)積分時積分系數(shù)Ki＝3，橫向位置偏差δb門限值εδ2＝0.150m，航向角偏差β門限值εβ＝0.06rad，橫向偏差初始值為δ0＝0m，航向角偏差初始值分別為β0＝－20°，β0＝－40°，β0＝－60°，轉(zhuǎn)向角初始值α0＝0°，車速vg＝8km/h，從圖3可以看出雙門限積分控制時的跟蹤效果明顯優(yōu)于常規(guī)積分控制時的跟蹤效果。

圖3 雙門限積分控制與常規(guī)積分控制跟蹤曲線比較圖Fig.3 Tracking curve comparison between double threshold integral control and conventional integral control

3 地下鏟運機自主避障行駛控制算法

將橫向位置偏差δb經(jīng)過限幅εδ1后再與航向角偏差β、航向角變化率偏差三種偏差進(jìn)行融合，形成融合偏差反饋e信號作為PID控制的輸入，在PID校正控制中，分別設(shè)置橫向位置偏差δb門限值εδ2及航向角偏差β門限值εβ，僅當(dāng)雙門限εδ2和εβ要求同時滿足時才進(jìn)行融合偏差反饋信號積分，PID校正后的輸出信號作為車輛轉(zhuǎn)向控制指令u，輸入給鏟運機的電液比例轉(zhuǎn)向控制系統(tǒng)，驅(qū)動車輛轉(zhuǎn)向并跟蹤目標(biāo)路徑，實現(xiàn)地下鏟運機自主行駛運動控制。

地下鏟運機自主行駛控制算法框圖見圖4。

圖4 地下鏟運機自主行駛控制算法框圖Fig.4 Underground loader autonomous driving control algorithm flow chart

該地下鏟運機自主避障行駛控制算法主要步驟如下：

1）在地下自主鏟運機主目標(biāo)路徑和局部避障目標(biāo)路徑規(guī)劃完成后，沿主目標(biāo)路徑曲線弧長sm（k）（k＝1，2，3，…）存儲主目標(biāo)路徑曲率Km（k）系列值，沿主目標(biāo)路徑曲線弧長sm（k）存儲局部避障目標(biāo)路徑的起始點smbb（i）和終止點smbe（i）（i為局部避障目標(biāo)路徑數(shù)量，i＝1，2，3，…），存儲局部避障目標(biāo)路徑相對于主目標(biāo)路徑的橫向偏離位移δb0（i），主目標(biāo)路徑曲率Km（k）及局部避障目標(biāo)路徑曲率Kmb（k）與主目標(biāo)路徑弧長sm（k）一一對應(yīng)，主目標(biāo)路徑和局部避障目標(biāo)路徑保存在自主行駛控制器的存儲介質(zhì)中。

2）用采樣周期T作為測量和控制間隔，當(dāng)車輛沿主目標(biāo)路徑行駛時，用車輛的定位參考點相對于主目標(biāo)路徑的位置P（sm（k），δ（k））和航向角偏差β（k），及車輛轉(zhuǎn)向角α（k），來表示車輛的位置。當(dāng)車輛沿局部避障目標(biāo)路徑行駛時，仍用車輛的定位參考點相對于主目標(biāo)路徑的位置P（sm（k），δ（k））和航向角偏差β（k），及車輛轉(zhuǎn)向角α（k），來表示車輛的位置。

3）給定Kp、Ki、Kd數(shù)值，給定T數(shù)值，給定l1、l2數(shù)值，給定K1、K2、K3數(shù)值，給定εδ1、εδ2、εβ數(shù)值。針對不同車輛上述可選參數(shù)的最佳值可由仿真計算結(jié)合實車實驗來得到。

4）輸入數(shù)據(jù)，δ（k）、δb0（i）、β（k）、sm（k）、smbb（i）、smbe（i）、α（k）、α（k－1）、vg（k）、Km（k）。

5）判斷鏟運機是否到達(dá)避障位置，當(dāng)鏟運機沒有到達(dá)局部避障目標(biāo)路徑位置時，輸入自主行駛控制器的橫向位置偏差為δb（k），δb（k）＝δ（k）；當(dāng)鏟運機到達(dá)局部避障目標(biāo)路徑位置時，自主行駛控制器輸入的橫向位置偏差為δb（k），δb（k）＝δ（k）＋δb0（i），δb0（i）為局部避障目標(biāo)路徑相對于主目標(biāo)路徑的橫向偏離位移。

1）局部避障目標(biāo)路徑的曲率Kmb（k）與主目標(biāo)路徑曲率Km（k）的關(guān)系為：

3）跟蹤主目標(biāo)路徑時，推算計算主目標(biāo)路徑投影車速vm（k）的公式為：

跟蹤局部避障目標(biāo)路徑時，推算計算局部避障目標(biāo)路徑投影車速vmb（k）的公式為：

4）跟蹤主目標(biāo)路徑及跟蹤局部避障目標(biāo)路徑時，推算計算目標(biāo)路徑航向角變化率的公式相同，均為：

5）跟蹤主目標(biāo)路徑及跟蹤局部避障目標(biāo)路徑時，計算航向角偏差變化率的公式為：

6）下一采樣時刻航向角偏差β（k＋1）可通過測量數(shù)據(jù)計算得出，也可通過計算下一采樣時刻航向角偏差β（k＋1）的公式計算得出：

7）下一采樣時刻橫向位置偏差δ（k＋1）及δb（k＋1）可通過測量數(shù)據(jù)計算得出，也可通過計算下一采樣時刻橫向位置偏差δ（k＋1）的公式推算得出：

8）計算目標(biāo)路徑弧長sm（k＋1）的公式為：

9）判斷橫向位置偏差δb（k）的數(shù)值，當(dāng)－εδ1≤δb（k）≤εδ1時，δb（k）＝δb（k）。當(dāng)δb（k）＞εδ1時，δb（k）＝εδ1。當(dāng)δb（k）＜－εδ1時，δb（k）＝－εδ1。

10）計算鏟運機橫向位置偏差δb（k）、航向角偏差β（k）及航向角偏差變化率三種偏差融合而成融合偏差反饋信號e（k），作為PID自主行駛控制器的輸入，計算融合反饋e（k）的公式為：

11）判斷橫向位置偏差｜δb（k）｜及航向角偏差｜β（k）｜的數(shù)值，當(dāng)｜δb（k）｜≤εδ2且｜β（k）｜≤εβ同時滿足時，PID自主行駛控制器才進(jìn)行融合偏差反饋信號e（k）的PID控制，輸出此時的轉(zhuǎn)向控制電壓u（k）。當(dāng)｜δb（k）｜≤εδ2和｜β（k）｜≤εβ不能同時滿足時，PID自主行駛控制器僅進(jìn)行融合偏差反饋信號的PD控制，輸出此時的轉(zhuǎn)向控制電壓u（k）。

融合反饋e（k）經(jīng)PID控制器校正后，計算輸出轉(zhuǎn)向控制電壓u（k）的方法為：

其中，u（k－1）為u（k）前一時刻控制電壓，Δu（k）為轉(zhuǎn)向控制電壓的增量，且Δu（k）＝Kp［e（k）－e（k－1）］＋εKie（k）＋Kd［e（k）－2e（k－1）＋e（k－2）］，其中，e（k－1）為e（k）前一時刻的融合反饋值，e（k－2）為e（k－1）前一時刻的融合反饋值，ε為積分項的開關(guān)系數(shù)：

12）轉(zhuǎn)向控制電壓u（k）作為地下鏟運機電液比例轉(zhuǎn)向控制系統(tǒng)的控制電壓，電液比例轉(zhuǎn)向控制系統(tǒng)驅(qū)動轉(zhuǎn)向油缸，實現(xiàn)鏟運機自主調(diào)整轉(zhuǎn)向角度，使鏟運機自動跟蹤目標(biāo)路徑。

13）當(dāng)采樣次數(shù)k＝1時，航向角偏差β（k），橫向位置偏差δ（k）和目標(biāo)路徑曲線弧長sm（k）都是實測值（或由實測值計算得到），航向角偏差變化率是計算值。當(dāng)k≥2時，航向角偏差β（k），橫向位置偏差δ（k），目標(biāo)路徑曲線弧長sm（k）既可以是實測值也可是推算值，航向角偏差變化率是計算值。

應(yīng)用該自主行駛控制及目標(biāo)路徑規(guī)劃方法，進(jìn)行地下鏟運機自主行駛控制仿真計算。取鏟運機前方驅(qū)動橋的中點到鉸接中心線的距離l1＝1.270m，取鏟運機后方驅(qū)動橋的中點到鉸接中心線的距離l2＝1.270m，取橫向位置偏差反饋系數(shù)K1＝0.20，取航向角偏差反饋系數(shù)K2＝0.55，取航向角偏差變化率反饋系數(shù)K3＝0.25，取PID的比例系數(shù)Kp＝30，取PID的微分系數(shù)Kd＝0，取PID的積分系數(shù)Ki＝18，取橫向位置偏差δb門限值εδ2＝0.150m，取航向角偏差β門限值εβ＝0.06rad，取車速vg＝4km/h，取鏟運機初始具有橫向偏差δ0＝1m，取航向角初始偏差分別為β0＝－30°，β0＝－15°，β0＝0°，β0＝＋15°，β0＝＋30°，取初始轉(zhuǎn)向角α0＝0°，目標(biāo)路徑為直線Km＝0m－1，鏟運機從原點（0，0）開始向右運行，路徑中包含一處避障路段A，A處避障距離δb0＝－3m，避障信號作用距離20m，起點為smbb（1）＝13m，終止點為smbe（1）＝33m，地下鏟運機在局部大尺寸避障時的跟蹤曲線圖見圖5，由圖中軌跡曲線可見鏟運機跟蹤與避障控制情況良好，自主行駛控制保持優(yōu)良的穩(wěn)定性和快速性。

圖5 地下鏟運機在局部大尺寸避障時的跟蹤曲線圖Fig.5 Tracking curves of underground loader in local large size obstacle avoidance

4 結(jié)論

1）針對巷道內(nèi)障礙物，自主行駛控制器僅需要存儲橫向位置避障偏離位移，及依附于主目標(biāo)路徑上的避障開始位置和避障結(jié)束位置數(shù)據(jù)，降低了自主行駛控制系統(tǒng)數(shù)據(jù)存儲量和管理難度，調(diào)整和組合這三個數(shù)值，就可組成無窮多種避障路徑，方便避障路徑規(guī)劃及避障運動控制。

2）三偏差通過非線性融合規(guī)則，形成融合偏差反饋信號，作為PID校正控制的輸入信號，橫向位置偏差通過限幅來慢速釋放大偏差對自主行駛控制器的過大作用量；橫向位置偏差及航向角偏差通過雙門限控制，使得融合偏差反饋信號獲得較理想的PID校正控制效果。

3）該自主行駛控制算法具有包括避障工況的跟蹤軌跡偏差預(yù)測功能，在鏟運機行駛過程中短時間缺少軌跡偏差實測值時，可推算出相對于主目標(biāo)路徑或局部避障目標(biāo)路徑的跟蹤軌跡偏差，使自主行駛不至中斷，該方法具有良好的適應(yīng)性和可靠性。

［1］戰(zhàn)凱 .地下遙控鏟運機遙控技術(shù)和精確定位技術(shù)研究［J］.有色金屬，2009，61（2）：107－111.

［2］高夢熊.采礦信息技術(shù)的現(xiàn)狀與發(fā)展［J］.礦山機械，2006，34（3）：45－51.

［3］遲洪鵬，戰(zhàn)凱，李建國，等 .地下自主鏟運機定位導(dǎo)航關(guān)鍵技術(shù)［J］.礦冶，2009，18（4）：57－60.

［4］石峰，顧洪樞，戰(zhàn)凱，等 .地下鏟運機跟蹤軌跡推算模型研究［J］.有色金屬，2010，62（6）：66－69.

［5］石峰，顧洪樞，戰(zhàn)凱，等 .地下無軌車輛自主導(dǎo)航控制器的研究［J］.礦冶，2010，19（4）：79－87.

［6］郭陽寬，王正林 .過程控制工程及仿真［M］.北京：電子工業(yè)出版社，2009.

［7］楊咸啟，常宗瑜 .機電工程控制基礎(chǔ)［M］.北京：國防工業(yè)出版社，2005.

Study on the control method of underground loader autonomous driving and obstacle avoidance

SHI Feng1，GU Hongshu1，ZHAN Kai1，LI Jianguo1，F(xiàn)ENG Xiaohua1，GUO Xin1，LI Hengtong1，CHEN Wei2，SHI Jinpeng3
（1.Beijing General Research Institute of Mining ＆Metallurgy，Beijing 100160，China；2.Dana Management（Shanghai）Co.，Ltd.，Shanghai 200443，China；3.Anshan Iron and Steel Group Corporation，Anshan Liaoning 114007，China）

Based on the measurement data or track prediction model，the lateral position deviation，course angle deviation and the change rate of course angle deviation are calculated.By nonlinear fusion rules，the fusion deviation feedback signal is formed as the input signal of PID tuning control.The lateral position deviation and double threshold value of course angle deviation are set to improve PID algorithm.The autonomous driving control method can still maintain excellent stability and rapidity when the track deviation and driving speed change in a large range.

underground loader；autonomous driving；obstacle avoidance；control algorithm

TD422.4

A

1671－4172（2015）05－0068－08

國家高技術(shù)研究發(fā)展計劃（863計劃）項目（2011AA060403）

石峰（1961－），男，教授級高級工程師，流體傳動與控制專業(yè)，主要從事地下無軌采礦設(shè)備研究開發(fā)。

10.3969/j.issn.1671－4172.2015.05.014