[發明專利]地下自主鏟運機的導航控制方法

有效

• 申請號: 200910238066.3
• 申請日: 2009-11-18
• 公開（公告）號: CN101713999A
• 公開（公告）日: 2010-05-26
• 發明（設計）人: 石峰;顧洪樞;戰凱;馮孝華;郭鑫;周俊武;李建國;劉立;馮茂林;陳樹新;遲洪鵬
• 申請（專利權）人: 北京礦冶研究總院
• 主分類號: G05D1/02
• 分類號: G05D1/02;G01C21/06
• 代理公司: 北京凱特來知識產權代理有限公司 11260
• 代理人: 鄭立明;趙鎮勇

1.一種地下自主鏟運機的導航控制方法，其特征在于，包括步驟：

A、以預先規劃好的鏟運機運動軌跡作為鏟運機的目標路徑，以鏟運機的定位參考點P的實際運動軌跡為鏟運機的跟蹤軌跡，沿所述目標路徑的曲線弧長s_m(k)存儲路徑曲率K_m(k)系列值和起始點航向角其中，k為采樣次數，k＝1，2，3，…，用采樣周期T作為測量和控制間隔；

以所述目標路徑上的航向角與所述跟蹤軌跡航向角的差值定義為航向角偏差用所述鏟運機的定位參考點P與該點投影于所述目標路徑上的點P_m的距離定義為鏟運機的橫向位置偏差δ；

B、用所述鏟運機的定位參考點P投影于所述目標路徑的點P_m的曲線弧長s_m(k)和鏟運機在該點的橫向位置偏差δ(k)作為所述鏟運機的相對位置P(s_m(k)，δ(k))；

用所述鏟運機的相對位置P(s_m(k)，δ(k))及鏟運機在該位置處的航向角偏差β(k)和鏟運機轉向角α(k)來表示所述鏟運機在該位置處的位姿；

所述鏟運機為前部與后部鉸接的結構，前部與后部的鉸接處安裝有角位移傳感器，所述角位移傳感器直接測量所述鏟運機轉向角α(k)；

C、通過所述鏟運機的定位參考點P的車速v_g(k)計算該點在所述目標路徑上的投影車速

計算所述目標路徑的航向角變化率和所述跟蹤軌跡的航向角變化率并計算航向角變化率偏差

所述目標路徑的航向角變化率的計算公式為：

所述跟蹤軌跡的航向角變化率的計算公式為：

式中，l₁為鏟運機前部驅動橋的中點到鉸接中心線的距離，l₂為鏟運機后部驅動橋的中點到鉸接中心線的距離；

D、由所述航向角變化率偏差航向角偏差β(k)和橫向位置偏差δ(k)三個參數構成綜合反饋e(k)，該綜合反饋用于鏟運機的轉向控制；

所述綜合反饋其中，K₁、K₂和K₃為預先存儲的反饋系數，K₁、K₂和K₃的選取范圍均為1～100。

2.根據權利要求1所述的地下自主鏟運機的導航控制方法，其特征在于，當鏟運機開始運行時，令采樣次數k＝1，輸入航向角偏差β(k)、橫向位置偏差δ(k)和目標路徑曲線弧長s_m(k)的直接測量值，鏟運機導航系統在此基礎上進行推算和控制；

當鏟運機運行一段時間后，再次令采樣次數k＝1，輸入航向角偏差β(k)、橫向位置偏差δ(k)和目標路徑曲線弧長s_m(k)新的直接測量值，鏟運機導航系統在此基礎上進行新的推算和控制。

3.如權利要求2所述的地下自主鏟運機的導航控制方法，其特征在于，所述步驟A中，所述目標路徑的路徑曲率K_m(k)系列值由鏟運機上裝備的陀螺儀和激光掃描器掃描巷道后，計算獲得。

4.如權利要求1所述的地下自主鏟運機的導航控制方法，其特征在于，所述目標路徑的路徑曲率K_m(k)與曲線弧長s_m(k)一一對應。

5.如權利要求1所述的地下自主鏟運機的導航控制方法，其特征在于，所述采樣周期T的選取范圍為，50毫秒≤T≤300毫秒。

6.如權利要求1所述的地下自主鏟運機的導航控制方法，其特征在于，所述目標路徑為記憶中的或由路徑產生模塊產生的虛擬路徑，保存在鏟運機導航控制器的存儲介質中。

7.如權利要求1所述的地下自主鏟運機的導航控制方法，其特征在于，所述綜合反饋e(k)經PID控制器校正后，輸出轉向控制電壓u(k)，該控制電壓u(k)用來控制鏟運機的電液比例轉向控制系統，實現鏟運機自主調整轉向角度，并自動跟蹤目標路徑。