本發明專利技術公開了一種地下鏟運機自主行駛和避障運動控制及目標路徑規劃方法，根據井下巷道情況，先規劃出主目標路徑，然后規劃出依附于主目標路徑的局部避障目標路徑，在鏟運機控制器中存儲主目標路徑數據，及局部避障目標路徑的起始點、終止點和相對于主目標路徑的偏移距離數據，由測量系統的測量數據或由軌跡推算模型，計算出的鏟運機橫向位置偏差、航向角偏差及航向角偏差變化率，三偏差通過融合規則形成綜合偏差反饋信號作為PID校正控制的輸入量，該控制指令輸入給鏟運機的電液比例轉向控制系統，驅動車輛轉向并跟蹤目標路徑，實現地下鏟運機自主導航和避障運動控制，方便、靈活、可靠。
【技術實現步驟摘要】
本專利技術涉及一種自主行駛和避障控制技術，尤其涉及一種。
技術介紹
隨著現有礦產資源逐步全面轉入地下開采，并向深部和難采礦體發展，開采規模逐漸增大，采礦條件愈來愈惡劣，對人的安全威脅也愈來愈大，遙控采礦技術應運而生。地下鏟運機是地下無軌采礦的關鍵設備，實現地下鏟運機在井下巷道內無人操縱、自主行駛，可使操作人員遠離井下惡劣、危險的工作環境，保護鏟運機司機人員安全、提高采礦作業效率并降低采礦成本。地下鏟運機自主行駛首先要解決自身在巷道內的自主行駛及避障運動控制問題及目標路徑規劃問題。地下鏟運機自主行駛研究在國內剛剛起步，國外在此方面已有較多的研究。如澳大利亞彼得里德利對鏟運機運動軌跡和軌跡控制進行了研究，構建了自主鏟運機自主行駛控制模型，該自主行駛控制模型在小曲率路徑跟蹤時誤差較小，但在目標路徑曲率較大時，跟蹤誤差較大，不能滿足控制精度要求。美國豐約翰對輪式機器人路徑跟蹤問題進行了研究，但將目標路徑局限于直線段和圓弧段，讓輪式機器人跟蹤每一個直線段或圓弧段，給出跟蹤直線段或圓弧段的計算辦法，該方法不能對其他形狀曲線路徑進行跟蹤計算，同時該研究也未涉及鉸接車輛情況，故無法應用于地下鏟運機。北京礦冶研究總院專利技術了一種基于車輛軌跡精確推算模型的地下鏟運機自主行駛控制方法，可較精確計算出鏟運機的跟蹤軌跡與目標路徑之間存在的偏差值，構成綜合偏差來控制鏟運機轉向并跟蹤目標路徑，該方法沒有考慮避障運動控制問題，另外，當跟蹤偏差較大時，控制系統的穩定性和控制精度會受到較大影響。所以，需要進一步研究解決地下鏟運機在較大偏差情況下，自主行駛控制器的穩定性問題，并提高自主行駛控制精度。避障運動控制是地下智能鏟運機一項必不可少的功能。現有避障技術是用幾段圓弧線組成的局部避障目標路徑，并與其他目標路徑相連接，形成一條連續的目標路徑，這樣做在理論上是可行的，但在實際應用時會很復雜，比如事先要存儲大量的目標路徑曲率，不同弧線段可能對應幾種不同的曲率需要存儲，同時還需要存儲不同弧線的切換點位置數據，這些情況帶來自主行駛避障控制系統數據管理難度大，而且避障目標路徑的規劃也很復雜，需要大量計算工作，實際避障控制時需要準確找到不同弧線的切換點，否則會嚴重影響避障控制效果，所以該方法應用時也很不方便；另外，在鏟運機鏟裝礦石出礦過程中，除了在巷道中心線位置鏟裝料堆中部的礦石外，鏟運機還需要偏左或偏右不斷變換位置去鏟裝料堆左右兩邊的礦石，通常每鏟運一次，換一個鏟取位置，這樣鏟取效率才高。要規劃出每一條偏離巷道主目標路徑的路徑數據量較大，也比較麻煩；為了降低井下巷道開拓成本，井下巷道的形狀和尺寸一般允許有一定程度的不規則情況存在，這給井下巷道目標路徑規劃帶來困難，并使自主行駛控制器存儲目標路徑數據量增多，目前也沒有解決這一問題的較好方法。
技術實現思路
本專利技術的目的是提供一種方便、靈活、可靠的。本專利技術的目的是通過以下技術方案實現的:本專利技術的，用來控制地下鏟運機跟蹤規劃出的主目標路徑和局部避障目標路徑，其特征在于，該方法包括以下主要步驟:A、根據地下巷道情況進行地下鏟運機主目標路徑和局部避障目標路徑規劃，在地下鏟運機主目標路徑和局部避障目標路徑規劃完成后，沿主目標路徑曲線弧長Sm(k)存儲主目標路徑曲率Km(k)系列值，沿主目標路徑曲線弧長Sm(k)存儲局部避障目標路徑的起始點smbb(i)和終止點Snte(i)及局部避障目標路徑相對于主目標路徑的橫向偏離位移δ b0 (i)，主目標路徑和局部避障目標路徑保存在自主行駛控制器的存儲介質中；上式中:k=l，2，3，…；i為局部避障目標路徑數量，i = 1，2，3，…；B、用采樣周期T作為測量和控制間隔，當車輛沿主目標路徑行駛時，用車輛的定位參考點相對于主目標路徑的位置P(sm(k)，δ (k))和航向角偏差β (k)，及車輛轉向角a (k)，來表示車輛的位姿；當車輛沿局部避障目標路徑行駛時，仍然使用車輛的定位參考點相對于主目標路徑的位置P(sm(k)，δ (k))和航向角偏差β (k)，及車輛轉向角a (k)，來表示車輛的位姿；C、判斷鏟運機是否到達避障位置，當鏟運機沒有到達局部避障目標路徑位置時，輸入自主行駛控制器的橫向位置偏差為sb(k)，sb(k) = s (k);當鏟運機到達局部避障目標路徑位置時，自主行駛控制器輸入的橫向位置偏差為Sb(k)，5b(k)=5 (k)+5b0(i),δΜ( )為局部避障目標路徑相對于主目標路徑的橫向偏離位移；D、計算鏟運機跟蹤軌跡航向角變化率Τ，計算目標路徑投影車速Vm(k)，計算目標路徑航向角變化率權利要求1.一種，用來控制地下鏟運機跟蹤規劃出的主目標路徑和局部避障目標路徑，其特征在于，該方法包括以下主要步驟: A、根據地下巷道情況進行地下鏟運機主目標路徑和局部避障目標路徑規劃，在地下鏟運機主目標路徑和局部避障目標路徑規劃完成后，沿主目標路徑曲線弧長Sm(k)存儲主目標路徑曲率Km (k)系列值，沿主目標路徑曲線弧長Sm (k)存儲局部避障目標路徑的起始點sfflbb(i)和終止點Snte(i)及局部避障目標路徑相對于主目標路徑的橫向偏離位移δΜα)，主目標路徑和局部避障目標路徑保存在自主行駛控制器的存儲介質中； 上式中:k=l，2，3，…；i為局部避障目標路徑數量，i=l，2，3，…； B、用采樣周期T作為測量和控制間隔，當車輛沿主目標路徑行駛時，用車輛的定位參考點相對于主目標路徑的位置P(sm(k)，δ (k))和航向角偏差β (k)，及車輛轉向角a (k)，來表示車輛的位姿；當車輛沿局部避障目標路徑行駛時，仍然使用車輛的定位參考點相對于主目標路徑的位置P(sm(k)，δ (k))和航向角偏差β (k)，及車輛轉向角a (k)，來表示車輛的位姿； C、判斷鏟運機是否到達避障位置，當鏟運機沒有到達局部避障目標路徑位置時，輸入自主行駛控制器的橫向位置偏差為Sb(k)，5b(k)=5 (k);當鏟運機到達局部避障目標路徑位置時，自主行駛控制器輸入的橫向位置偏差為Sb(k)，5b(k)=5 (k)+5b0(i), 5b0(i)為局部避障目標路徑相對于主目標路徑的橫向偏離位移； D、計算鏟運機跟蹤軌 跡航向角變化率計算目標路徑投影車速Vm(k)，計算目標路徑航向角變化率^計算航向角偏差變化率f，計算下一采樣時刻航向角偏差β (k+1)，計算下一采樣時刻橫向位置偏差δ (k+1)及Sb(k+1)，計算下一采樣時刻目標路徑曲線弧長Sm(k+Ι)，由目標路徑曲率存儲控制子系統得到下一采樣時刻目標路徑曲率Km (k+1)及 Kmb (k+1)的數值； E判斷橫向位置偏差5b(k)的數值，當_ε δ1^ 5b(k)彡ε S1時，5b(k)=5b(k)0當δ b(k) > ε S1 時，δ b (k) = ε Sl ;當 δ b (k) < - ε Sl 時，δ b (k) =- ε Sl,其中 ε δ1 的取值范圍為，O < ε S1 彡 I (m)； F、計算鏟運機橫向位置偏差Sb(k)、航向角偏差β(k)及航向角偏差變化率T三種偏差融合而成綜合偏差反饋信號e (k)，作為PID校正控制器的輸入； G、判斷橫向位置偏差ISb(k) I本文檔來自技高網...
【技術保護點】
一種地下鏟運機自主行駛和避障運動控制及目標路徑規劃方法，用來控制地下鏟運機跟蹤規劃出的主目標路徑和局部避障目標路徑，其特征在于，該方法包括以下主要步驟：A、根據地下巷道情況進行地下鏟運機主目標路徑和局部避障目標路徑規劃，在地下鏟運機主目標路徑和局部避障目標路徑規劃完成后，沿主目標路徑曲線弧長sm(k)存儲主目標路徑曲率Km(k)系列值，沿主目標路徑曲線弧長sm(k)存儲局部避障目標路徑的起始點smbb(i)和終止點smbe(i)及局部避障目標路徑相對于主目標路徑的橫向偏離位移δb0(i)，主目標路徑和局部避障目標路徑保存在自主行駛控制器的存儲介質中；上式中：k=1，2，3，…；i為局部避障目標路徑數量，i=1，2，3，…；B、用采樣周期T作為測量和控制間隔，當車輛沿主目標路徑行駛時，用車輛的定位參考點相對于主目標路徑的位置P(sm(k),δ(k))和航向角偏差β(k)，及車輛轉向角α(k)，來表示車輛的位姿；當車輛沿局部避障目標路徑行駛時，仍然使用車輛的定位參考點相對于主目標路徑的位置P(sm(k),δ(k))和航向角偏差β(k)，及車輛轉向角α(k)，來表示車輛的位姿；C、判斷鏟運機是否到達避障位置，當鏟運機沒有到達局部避障目標路徑位置時，輸入自主行駛控制器的橫向位置偏差為δb(k)，δb(k)=δ(k)；當鏟運機到達局部避障目標路徑位置時，自主行駛控制器輸入的橫向位置偏差為δb(k)，δb(k)=δ(k)+δb0(i)，δb0(i)為局部避障目標路徑相對于主目標路徑的橫向偏離位移；D、計算鏟運機跟蹤軌跡航向角變化率計算目標路徑投影車速vm(k)，計算目標路徑航向角變化率計算航向角偏差變化率計算下一采樣時刻航向角偏差β(k+1)，計算下一采樣時刻橫向位置偏差δ(k+1)及δb(k+1)，計算下一采樣時刻目標路徑曲線弧長sm(k+1)，由目標路徑曲率存儲控制子系統得到下一采樣時刻目標路徑曲率Km(k+1)及Kmb(k+1)的數值；E判斷橫向位置偏差δb(k)的數值，當?εδ1≤δb(k)≤εδ1時，δb(k)=δb(k)。當δb(k)＞εδ1時，δb(k)=εδ1；當δb(k)＜?εδ1時，δb(k)=?εδ1，其中εδ1的取值范圍為，0＜εδ1≤1（m）；F、計算鏟運機橫向位置偏差δb(k)、航向角偏差β(k)及航向角偏差變化率三種偏差融合而成綜合偏差反饋信號e(k)，作為PID校正控制器的輸入；G、判斷橫向位置偏差|δb(k)|及航向角偏差|β(k)|的數值，當|δb(k)|≤εδ2且|β(k)|≤εβ同時滿足時，PID自主行駛控制器才進行綜合偏差反饋信號e(k)的PID控制，此時PID的積分環節起作用，輸出此時的轉向控制電壓u(k);當|δb(k)|≤εδ2和|β(k)|≤εβ不能同時滿足時，PID自主行駛控制器僅進行綜合偏差反饋信號的PD控制，此時PID的積分環節不起作用，輸出此時的轉向控制電壓u(k)；H、轉向控制電壓u(k)作為鏟運機電液比例轉向控制系統的控制電壓，電液比例轉向控制系統驅動轉向油缸，實現鏟運機自主調整轉向角度，使鏟運機行駛軌跡自動跟蹤目標路徑；I、當采樣次數k=1時，航向角偏差β(k)，橫向位置偏差δ(k)和目標路徑曲線弧長sm(k)都是實測值或由實測值計算得到，航向角偏差變化率是計算值；當k≥2時，航向角偏差β(k)，橫向位置偏差δ(k)，目標路徑曲線弧長sm(k)既可以是實測值也可是推算值，航向角偏差變化率是計算值。FDA00002914926100011.jpg,FDA00002914926100012.jpg,FDA00002914926100013.jpg,FDA00002914926100014.jpg,FDA00002914926100021.jpg,FDA00002914926100022.jpg...
【技術特征摘要】
【專利技術屬性】
技術研發人員：石峰，顧洪樞，戰凱，馮孝華，郭鑫，李建國，段辰玥，陳強，
申請(專利權)人：北京礦冶研究總院，北礦機電科技有限責任公司，
類型：發明
國別省市：
還沒有人留言評論。發表了對其他瀏覽者有用的留言會獲得科技券。