[發明專利]一種基于LOS理論的農機路徑跟蹤方法

有效

• 申請號: 202110561035.2
• 申請日: 2021-05-21
• 公開（公告）號: CN113359710B
• 公開（公告）日: 2022-11-18
• 發明（設計）人: 丁世宏;劉壯壯;魏新華;劉陸
• 申請（專利權）人: 江蘇大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于LOS理論的農機路徑跟蹤方法，其特征在于，包括以下步驟：

S1，農機在已設定的直線路徑上行駛時，由導航定位模塊獲取農機實時位置信息；

S2，將位置信息通過上位機系統解析的坐標、橫向跟蹤誤差等參數傳送到LOS算法器中得出當前行駛的航向角；

S3，前輪反饋控制器根據航向角和橫向跟蹤誤差，輸出系統期望的車輪轉向角度，由此可以得出車輪的轉向偏差角，重復以上步驟，實現路徑跟蹤控制。

2.根據權利要求1所述的基于LOS理論的農機路徑跟蹤方法，其特征在于，所述步驟S1具體包括：

S11，通過北斗RTK定位模塊和視覺識別CDD模塊設定農機工作田塊邊界，進而打點出若干條平行的直線軌跡。

S12，北斗RTK導航模塊實時輸出農機的位置，軌跡點信息，同時與矩形田塊的三個頂點坐標轉化到高斯平面坐標系中，并將規劃的路徑以二維數組形式儲存到導航系統中，實現導航路徑規劃。

3.根據權利要求2所述的基于LOS理論的農機路徑跟蹤方法，其特征在于，所述步驟S11具體為：

利用RTK-CCD定位視覺傳感器融合系統，測定試驗田三個坐標頂點確定一個矩形，假設為地頭節點A、B、C、D定位坐標；以每條作業行距離最長為原則，結合地塊實際幾何形狀，選取邊界AD作為作業行規劃的基準線，以其為基準劃定若干條平行作業線，平行線距離依作業行間距而定，最后一條作業線與邊界BC的最短距離不得小于1/2個作業間距，將所有Q條平行作業線與邊界AB和CD的焦點，作為規劃作業線節點，并將相應作業線節點存進二維數組中，供基于LOS算法器的導航控制軟件調用。

4.根據權利要求1所述的基于LOS理論的農機路徑跟蹤方法，其特征在于，所述步驟S2具體包括：

S21：確定一條由兩個路徑點和定義的直線跟蹤路徑，并且路徑坐標系的原點在農機在路徑固定的坐標系內坐標為pⁿ(t)＝[x(t),y(t)]，且滿足公式：

式中：

α_k＝a tan2(y_k+1-y_k,x_k+1-x_k)∈S；α_k為大地坐標系正北方與期望路徑P_k和P_k+1的夾角，a為系數；

S22：令ε(t)＝[s(t),e(t)]^T∈R²，其中：

s(t)＝[x(t)-x_k]cos(α_k)+[y(t)-y_k]sin(α_k)

e(t)＝-[x(t)-x_k]sin(α_k)+[y(t)-y_k]cos(α_k)

s(t)為農機路徑跟蹤距離，e(t)為橫向跟蹤誤差，在實際路徑跟蹤過程中，只需要關注農機本身行駛的橫向偏差，因為當橫向偏差e(t)＝0時，意味著農機已經收斂于期望跟蹤路徑上，路徑跟蹤控制目標即為：

S23：設定LOS導航算法為：

式中：

為速度—路徑相關角，其中Δ為農機的前視距離，e為橫向跟蹤誤差；

若農機當行駛方向角為則航向角為：