[發明專利]用于農機無人駕駛的路徑規劃及其控制方法

在審

• 申請號: 201710156169.X
• 申請日: 2017-03-16
• 公開（公告）號: CN106909151A
• 公開（公告）日: 2017-06-30
• 發明（設計）人: 程方;林德相;袁洪良;王爍;徐濤;王浩;吳飛;奚小波
• 申請（專利權）人: 無錫卡爾曼導航技術有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 南京蘇科專利代理有限責任公司32102
• 代理人: 沈良菊

1.一種用于農機無人駕駛的路徑規劃及其控制方法，其特征在于，

步驟1：通過傳感器獲取農機環境信息做出掉頭或避障決策；若為避障決策，規劃避障路徑軌跡，具體包括以下步驟，

步驟101：使用改進的最短切線法離線計算出一條理論避障路徑；

步驟102：利用基于Bezier曲線的路徑優化方法優化步驟101中的理論避障路徑得到實際避障路徑；

若為掉頭決策，規劃掉頭路徑軌跡，具體包括以下步驟，

步驟101’：輸入農機的作業模式；

步驟102’：根據農機實際的作業模式選擇對應的掉頭路徑；

步驟2：農機在前進過程中，農機通過傳感器獲得農機位置信息，使用曲線跟蹤方法獲得實時曲線曲率和實時的農機航向偏差、橫向偏差，用狀態反饋控制器和自適應控制器的結合計算當前的前輪轉向角，控制農機的轉向角實現農機的自動掉頭或避障。

2.根據權利要求1所述的用于農機無人駕駛的路徑規劃及其控制方法，其特征在于，步驟101中，計算理論避障路徑具體的為，計算農機前方障礙物的特征圓的大小、農機與障礙物的距離，根據特征圓的大小設定安全距離，根據農機的犁具寬度和農機最小轉彎半徑，設定一條理論避障路徑；改進的最短切線法具體的為，以障礙物的中心為圓心做特征圓，特征圓的半徑為r_min+w/2，理論避障路徑由圓弧段一、直線段一、圓弧段二、直線段二和圓弧段三組成，圓弧段一的一端與農機原始的直線路徑相切，圓弧段一的另一端與直線段一的一端相切，直線段一的另一端和直線段二的一端分別與圓弧段二相切，直線段二的另一端與圓弧段三相切，圓弧段二為特征圓上的一段，圓弧段一和圓弧段三關于圓弧段二的中心線對稱設置，農機依次經過圓弧段一、直線段一、圓弧段二、直線段二和圓弧段三繞過障礙物；其中，r_min為農機的最小轉彎半徑，w為農機的作業寬度，障礙物的外接圓半徑小于最小轉彎半徑。

3.根據權利要求2所述的用于農機無人駕駛的路徑規劃及其控制方法，其特征在于，所述圓弧段一的半徑為r_min，所述圓弧段三的半徑為r_min，圓弧段一的起點記為H點，圓弧段一的圓心記為O₁點，直線段一與農機原始的直線路徑的相交點記為J，直線段一與圓弧段二的相切點記為D，農機原始路徑與特征圓的相交點分別記為K和K’，JK=w/2，圓弧段二的圓心記為O點，O的坐標設為（a，b），圓弧段二的中心點記為B點，J點的坐標記為（x1，y1），JD的方程可以寫成：

（1-1）；

特征圓的方程可以寫成：

（1-2）

通過（1-1）和（1-2）可以求出k，D點為JD和特征圓的相交點，以此解出D點坐標；

設點O₁的坐標為（x₂,y₂），則點O₁到直線JD的距離為：

根據公式（1-3）和（1-4）求出O₁的坐標；則H點的坐標為（x₂，y₁），B點的坐標為（a，b+r）。