用于農機無人駕駛的田間掉頭路徑規劃及其控制方法與流程
本發明涉及一種自動掉頭路徑規劃及其控制方法，特別涉及一種用于農機無人駕駛的田間掉頭路徑規劃及其控制方法。
背景技術：
農機在導航作業時一般需要進行自動掉頭對準下一作業行的跟蹤，控制農機準確的掉頭技術是實現精準農業的關鍵，該技術可提高農機的作業精度和作業效率，使駕駛員擺脫長時間勞累的重復駕駛工作，降低勞動力。
在現有的農機路徑跟蹤方法上，控制農機按照設定好的路徑行走的控制方法有多種，如BUG算法、人工勢場法、VFH算法、模糊邏輯算法、模糊神經網絡算法等，這些算法的應用場景均是復雜的作業環境，算法的邏輯復雜， 應用于農機作業不復雜的作業環境下，反應速度較慢，反而降低其控制精度；另外，對于由若干個大小不同的田塊組成的田地，工作人員需攜帶多個導航系統，到一個田地工作時，工作人員需根據田塊的實際大小選擇一個與之對應的導航系統安裝到農機上，到另一塊寬幅不同的田塊時，將以上導航系統拆下來，將新的導航系統重新安裝到農機上，無法根據農機的實際作業模式進行路徑控制，適用范圍小，成本高，操作麻煩。
技術實現要素：
針對現有技術中的缺陷，本發明的目的在于克服上述現有技術中的不足之處，解決現有技術中控制精度低且適用范圍小的技術問題，提供一種用于農機無人駕駛的田間掉頭路徑規劃及其控制方法，本發明的控制精度高，適用范圍廣。
本發明的目的是這樣實現的：一種用于農機無人駕駛的田間掉頭路徑規劃及其控制方法，具體包括以下步驟，
步驟1：輸入農機的作業模式；
步驟2：獲取農機所處環境的地理信息，根據農機實際的作業模式選擇對應的掉頭路徑；
步驟3：農機在前進過程中，農機通過傳感器獲得農機位置信息，使用預瞄和PI控制器組合起來計算當前的前輪轉向角，控制農機的轉向角實現農機的自動掉頭。
本發明工作時，工作人員先根據田塊的寬幅輸入農機的作業模式，通過傳感器感應農機周圍的環境信息，農機掉頭時，農機根據實際的作業模式選擇設定好的掉頭路徑，農機通過傳感器檢測獲得農機的位置信息，使用預瞄控制器和PI控制器相結合起來實時計算出農機的前輪轉角，農機在行走過程中通過實時調整農機的前輪轉角，通過控制農機的前輪轉向角使農機沿著設定的曲線行走，從而實現農機的掉頭；本發明中設定了不同作業模式下對應的掉頭路徑，可通過農機的實際作業模式選擇與其相對應的掉頭路徑，集成度高，適用范圍更加廣泛，降低成本，操作簡便；使用預瞄控制器和PI控制器的結合控制農機的前輪轉向角使農機沿著設定好的避障曲線行走，控制精度高；本發明可應用于農機在導航作業時的自動掉頭的工作中。
為了進一步提高農機掉頭速度，本發明的步驟1中，農機的作業模式包括播種和耕地；步驟2中，播種情況下對應的掉頭路徑為多線型路徑一，耕地情況下對應的掉頭路徑包括交叉型路徑、圓弧路徑和多線型路徑二，且根據不同的犁具寬度選擇不同的掉頭路徑，犁具寬度包括小寬幅、中寬幅和大寬幅，小寬幅、中寬幅和大寬幅作業模式下對應的掉頭路徑分別為交叉型路徑、圓弧路徑和多線型路徑二，小寬幅對應的犁具寬度的數值范圍為Rmin<W<1.5Rmin,中寬幅對應的犁具寬度的數值范圍為1.5Rmin<W≤2Rmin,大寬幅對應的犁具寬度的數值范圍為W>2Rmin，Rmin為農機的最小轉彎半徑，W為犁具寬度；此設計中，將農機的作業模式分為播種和耕地，不同作業模式，對應的掉頭路徑不同，優化掉頭曲線，縮短農機掉頭時間。
為了進一步提高農機掉頭效率，所述多線型路徑一由直線段一、圓弧段一和圓弧段二組成，圓弧段一和圓弧段二相切且對應的半徑相同，農機依次經過直線段一、圓弧段一和圓弧段二實現掉頭；所述交叉型路徑由圓弧段三、直線段二和圓弧段四組成，圓弧段三和圓弧段四相交且關于直線段一的中心對稱，圓弧段三和圓弧段四的半徑大小相同，農機依次經過圓弧段三、直線段二和圓弧段四實現掉頭；所述圓弧路徑由圓弧段五、圓弧段六和圓弧段七組成，所述圓弧段六分別和圓弧段五、圓弧段七相切，所述圓弧段五和圓弧段七關于圓弧段六的中心線對稱設置，農機依次經過圓弧段五、圓弧段六和圓弧段七實現掉頭；所述多線型路徑二由圓弧段八、直線段三和圓弧段九組成，圓弧段八和圓弧段九關于直線段三的中心線對稱設置，農機依次經過圓弧段八、直線段九和圓弧段九實現掉頭。
為了進一步提高農機掉頭路徑的精度，所述多線型路徑一的各個線段的數學關系式為：
（1-1）
其中，R1為圓弧段一對應的半徑，R2為圓弧段二對應的半徑，AE為直線段一的長度，AC為農機開始轉彎的后軸中心與農機轉彎結束的后軸中心間連接線段的長度；
所述交叉型路徑一的各個線段的數學關系式為：
（1-2）
其中，R3為圓弧段三對應的半徑，R4為圓弧段四對應的半徑，C1D1為直線段二的長度，A1B1農機開始轉彎的后軸中心與農機轉彎結束的后軸中心間連接線段的長度；
所述圓弧路徑的各個線段的數學關系式為：
（1-3）
其中，Rt為圓弧段五、圓弧段六和圓弧段七對應的半徑，圓弧段五、圓弧段六和圓弧段七的圓心的連線構成三角形，β為圓弧段六的圓心分別與圓弧段五和圓弧段七的圓心連線之間的夾角，α為圓弧段五的圓心分別與圓弧段六和圓弧段七的圓心連線之間的夾角，γ為圓弧段七的圓心分別與圓弧段五和圓弧段六的圓心連線之間的夾角；
所述多線型路徑二的各個線段的數學關系式為：
（1-4）
其中，R5為圓弧段八對應的半徑，R6為圓弧段九對應的半徑，B2C2為直線段三的長度，A2D2為圓弧段八的圓心和圓弧段九的圓心的連線；
此設計中，根據不同作業模式建立不同的路徑模型，為準確跟蹤曲線建立基礎。
為了提高跟蹤曲線的控制精度，步驟3中，利用預瞄控制器計算出理論前輪轉角，具體的為，確定農機的前視距離l，取路徑上的一點為預瞄點（x0，y0），R為前視距離對應的圓弧段的半徑， l、R和x之間的關系式為：
（2-1）
將農機簡化為二輪車，建立農機的運動學模型：
（2-2）
根據阿克曼轉向幾何關系，農機的轉彎半徑和前輪轉角、軸距的關系式為：
（2-3）
將（3-2）和（3-3）結合起來得到理論轉角的計算公式為：
（2-4）
其中，θ為農機的航向偏差角，農機后軸中心記為點A，農機后軸中心A和預瞄點P連線記為AP，航向偏差角為農機航向與AP之間的夾角，δ為農機的理論前輪轉角，L為農機的軸距，v為農機的行駛速度，x0、y0為農機的橫坐標和縱坐標，設定的曲線路徑上距離農機中心最近的點即為M。
為了進一步提高跟蹤曲線的控制精度，步驟3中，使用PI控制方法計算出補償前輪轉角，具體的包括以下步驟：
（301）根據農機的位置和預瞄點計算出農機的航向偏差角θ作為PI的誤差輸入e（k）；
（302）計算出當前的積分累計誤差；
（303）PI控制輸出補償前輪轉角，補償前輪轉角的計算公式為：
（3）
其中，Kp為比例增益，Ki為積分增益，e（i）為i時間點下對應的誤差輸入，k為總采樣時間點數，u（k）為PI控制的輸出，具體的為當前的補償前輪轉角；
此設計中，考慮到農機在實際運行中，轉向關系并不是完全滿足預瞄控制器中的阿克曼轉向原理，會存在一定的控制誤差，將PI控制方法對預瞄控制方法進行輔助控制，消除預瞄控制器帶來的誤差，進一步提高控制精度。
作為本發明的進一步改進，步驟3中，使用預瞄和PI控制器組合起來計算當前的前輪轉向角具體的為，通過預瞄控制器計算得到理論前輪轉角，PI控制器計算得到補償前輪轉角，將理論前輪轉角和補償前輪轉角相加后得到實際前輪轉角，將實際前輪轉角實時輸出給農機模型，控制農機的前輪轉角實現農機的自動避障。
作為本發明的進一步改進，所述傳感器包括位置傳感器和角度傳感器，所述角度傳感器檢測農機的轉向角，所述位置傳感器獲得農機的位置信息；農機的前后側分別設有視覺機器相機，所述視覺機器相機獲取農機所處環境的地理信息。
附圖說明
圖1為本發明中播種時的掉頭路徑軌跡圖。
圖2為本發明中耕地時小幅寬的掉頭路徑軌跡圖。
圖3為本發明中耕地時中幅寬的掉頭路徑軌跡圖。
圖4為本發明中耕地時大幅寬的掉頭路徑軌跡圖。
圖5為本發明的控制框圖。
圖6為本發明中的預瞄算法示意圖。
圖7為本發明中PI控制的曲線跟蹤示意圖。
圖8為本發明中設定曲線與跟蹤曲線的仿真模擬曲線跟蹤對比圖。
圖9為本發明中的航向偏差模擬圖。
圖10為本發明中的橫向偏差模擬圖。
圖11為本發明中的前輪轉向角曲線圖。
具體實施方式
下面結合附圖對本發明作進一步的說明。
如圖1～11所示的一種用于農機無人駕駛的田間掉頭路徑規劃及其控制方法，具體包括以下步驟：
步驟1：輸入農機的作業模式；
步驟2：獲取農機所處環境的地理信息，根據農機實際的作業模式選擇對應的掉頭路徑；
步驟3：農機在前進過程中，農機通過傳感器獲得農機位置信息，使用曲線跟蹤方法獲得實時曲線曲率、航向偏差和橫向偏差，用狀態反饋控制器和自適應控制器的結合計算當前的前輪轉向角，控制農機的轉向角實現農機的自動掉頭；
本發明的步驟1中，農機的作業模式包括播種和耕地；另外，步驟1中的傳感器包括位置傳感器和角度傳感器，角度傳感器檢測農機的轉向角，位置傳感器獲得農機的位置信息；農機的前后側分別設有視覺機器相機，視覺機器相機獲取農機所處環境的地理信息；
本發明的步驟2中，播種情況下對應的掉頭路徑為多線型路徑一，如圖1所示，多線型路徑一由直線段一l1、圓弧段一ρ1和圓弧段二ρ2組成，圓弧段一ρ1和圓弧段二ρ2相切且對應的半徑相同，農機依次經過直線段一l1、圓弧段一ρ1和圓弧段二ρ2實現掉頭；耕地情況下，根據不同的犁具寬度設置不同的掉頭路徑，犁具寬度包括小寬幅、中寬幅和大寬幅，小寬幅對應的犁具寬度的數值范圍為Rmin<W<1.5Rmin,中寬幅對應的犁具寬度的數值范圍為1.5Rmin<W≤2Rmin,大寬幅對應的犁具寬度的數值范圍為W>2Rmin，Rmin為農機的最小轉彎半徑，W為犁具寬度；小寬幅、中寬幅和大寬幅下對應的掉頭路徑分別為交叉型路徑、圓弧路徑和多線型路徑二，交叉型路徑（如圖2所示）由圓弧段三ρ3、直線段二l2和圓弧段四ρ4組成，圓弧段三ρ3和圓弧段四ρ4相交且關于直線段二l2的中心對稱設置，圓弧段三ρ3和圓弧段四ρ4的半徑大小相同，農機依次經過圓弧段三ρ3、直線段二l2和圓弧段四ρ4實現掉頭；圓弧路徑（如圖3所示）由圓弧段五ρ5、圓弧段六ρ6和圓弧段七ρ7組成，圓弧段六ρ6分別和圓弧段五ρ5、圓弧段七ρ7相切，圓弧段五ρ5和圓弧段七ρ7關于圓弧段六ρ6的中心線對稱設置，農機依次經過圓弧段五ρ5、圓弧段六ρ6和圓弧段七ρ7實現掉頭；多線型路徑二（如圖4所示）由圓弧段八ρ8、直線段三l3和圓弧段九ρ9組成，圓弧段八ρ8和圓弧段九ρ9關于直線段三l3的中心線對稱設置，農機依次經過圓弧段八ρ8、直線段三l3和圓弧段九ρ9實現掉頭；
其中，多線型路徑一的各個線段的數學關系式為：
（1-1）
其中，R1為圓弧段一ρ1對應的半徑，R2為圓弧段二ρ2對應的半徑，AE為直線段一l1的長度，AC為農機開始轉彎的后軸中心與農機轉彎結束的后軸中心間連接線段的長度；
交叉型路徑一的各個線段的數學關系式為：
（1-2）
其中，R3為圓弧段三ρ3對應的半徑，R4為圓弧段四ρ4對應的半徑，C1D1為直線段二l2的長度，A1B1農機開始轉彎的后軸中心與農機轉彎結束的后軸中心間連接線段的長度；
圓弧路徑的各個線段的數學關系式為：
（1-3）
其中，Rt為圓弧段五ρ5、圓弧段六ρ6和圓弧段七ρ7對應的半徑，圓弧段五ρ5、圓弧段六ρ6和圓弧段七ρ7的圓心的連線構成三角形，β為圓弧段六ρ6的圓心分別與圓弧段五ρ5和圓弧段七ρ7的圓心連線之間的夾角，α為圓弧段五ρ5的圓心分別與圓弧段六ρ6和圓弧段七ρ7的圓心連線之間的夾角，γ為圓弧段七ρ7的圓心分別與圓弧段五ρ5和圓弧段六ρ6的圓心連線之間的夾角；
多線型路徑二的各個線段的數學關系式為：
（1-4）
其中，R5為圓弧段八ρ8對應的半徑，R6為圓弧段九ρ9對應的半徑，B2C2為直線段三l3的長度，A2D2為圓弧段八ρ8的圓心和圓弧段九ρ9的圓心的連線；
步驟3中，利用預瞄控制器計算出理論前輪轉角，具體的為，確定農機的前視距離l，取路徑上的一點為預瞄點（x0，y0），R為前視距離對應的圓弧段的半徑， l、R和x之間的關系式為：
（2-5）
由（3-5）可以得到：
（2-1）
將農機簡化為二輪車，建立農機的運動學模型：
（2-2）
根據阿克曼轉向幾何關系，農機的轉彎半徑和前輪轉角、軸距的關系式為：
（2-3）
將（3-2）和（3-3）結合起來得到理論轉角的計算公式為：
（2-4）
其中，θ為農機的航向偏差角，農機后軸中心記為點A，農機后軸中心A和預瞄點P連線記為AP，航向偏差角為農機航向與AP之間的夾角，δ為農機的理論前輪轉角，L為農機的軸距，v為農機的行駛速度，x0、y0為農機的橫坐標和縱坐標，設定的曲線路徑上距離農機中心最近的點即為M，D為圓弧段的圓心與預瞄點P橫坐標間的橫向距離；為農機在x軸方向上的速度，為農機在y軸方向上的速度，v為農機的速度。
考慮到農機在實際運行中，轉向關系并不是完全滿足預瞄控制器中的阿克曼轉向原理，會存在一定的控制誤差，將PI控制方法對預瞄控制方法進行輔助控制，消除預瞄控制器帶來的誤差，進一步提高控制精度，其中，使用PI控制方法計算出補償前輪轉角，具體的包括以下步驟：
（301）根據農機的位置和預瞄點計算出農機的航向偏差角θ作為PI的誤差輸入e（k）；
（302）計算出當前的積分累計誤差；
（303）PI控制輸出補償前輪轉角，補償前輪轉角的計算公式為：
（3）
其中，Kp為比例增益，Ki為積分增益，e（i）為i時間點下對應的誤差輸入，k為總采樣時間點數，u（k）為PI控制的輸出，具體的為當前的補償前輪轉角；
步驟3中，使用預瞄和PI控制器組合起來計算當前的前輪轉向角具體的為，通過預瞄控制器計算得到理論前輪轉角，PI控制器計算得到補償前輪轉角，將理論前輪轉角和補償前輪轉角相加后得到實際前輪轉角，將實際前輪轉角實時輸出給農機模型，控制農機的前輪轉角實現農機的自動避障。
使用matlab軟件對本發明進行仿真，設定由直線和圓弧構成的曲線，給定農機的起始位置為（-13,1），初始航向角度為0rad，Kp取為2，Ki取為0.01；利用本發明的控制方法對設定好的曲線進行跟蹤，圖8～圖11的橫坐標均為農機的行駛距離，從圖8中可以看出，跟蹤曲線與設定曲線基本重合；從圖9中可以看出，橫向偏差維持在10cm左右；從圖10中可以看出，航向偏差大概在0.08rad左右；從圖11中可以看出，前輪轉向角為1階慣性環節，沒有突變，和實際相符；通過以上分析，使用本發明中的控制方法進行農機的掉頭路徑控制，控制精度高，農機基本按照設定的曲線路徑行走。
本發明工作時，輸入農機的作業模式，視覺機器相機采集農機周圍的環境信息，根據周圍環境信息確認農機是否掉頭；農機掉頭時，農機根據實際的作業模式選擇設定好的掉頭路徑，當作業模式為播種時，農機的掉頭路徑選擇多線型路徑一，農機通過傳感器檢測獲得農機的位置信息，通過預瞄控制器得到理論前輪轉角，PI控制器補償預瞄控制器產生的控制誤差輸出補償前輪轉角，將理論前輪轉向角和期望補償轉向角相加后得到實際前輪轉角并將前輪轉角輸出給農機模型，位置傳感器實時檢測農機所在位置并將位置信息發送給預瞄控制器和PI控制器，通過控制農機的前輪轉角使農機沿著設定的曲線行走，從而實現農機的掉頭；當作業模式為耕地時，根據農機上懸掛的犁具寬度選擇掉頭路徑，掉頭路徑軌跡選定后，按照上述相同的控制過程控制農機的掉頭；本發明中設定了不同作業模式下對應的掉頭路徑，可通過農機的實際作業模式選擇與其相對應的掉頭路徑，集成度高，適用范圍更加廣泛，降低成本，操作簡便；使用預瞄控制器和PI控制器的結合控制農機的前輪轉向角使農機沿著設定好的避障曲線行走，控制精度高；本發明可應用于農機在導航作業時的自動掉頭的工作中。
本發明并不局限于上述實施例，在本發明公開的技術方案的基礎上，本領域的技術人員根據所公開的技術內容，不需要創造性的勞動就可以對其中的一些技術特征作出一些替換和變形，這些替換和變形均在本發明保護范圍內。
- 還沒有人留言評論。精彩留言會獲得點贊！