[發明專利]一種高精度循跡和控制的巡邏車路徑規劃及跟蹤算法

在審

• 申請號: 202111096027.1
• 申請日: 2021-09-18
• 公開（公告）號: CN113867341A
• 公開（公告）日: 2021-12-31
• 發明（設計）人: 夏勇;周曉宇;陳傳飛;薛巨峰;范東睿
• 申請（專利權）人: 鹽城中科高通量計算研究院有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 南京眾聯專利代理有限公司 32206
• 代理人: 葉涓涓

1.一種高精度循跡和控制的巡邏車路徑規劃及跟蹤算法，具體步驟如下，其特征在于：

步驟1，柵格化巡邏車使用地的地理：采集巡邏車行駛場地的數據，根據場地數據將場地地圖柵格化，柵格塊設置為正方形，橫縱向的柵格移動代價設為10，對角線的柵格移動代價設為14，每個柵格都對應一個地理中的坐標范圍，同時將柵格分為空區域、障礙內區域和目標區域；

步驟2，設計巡邏車路徑規劃模型：將起點和坐標加入模型中，同時遍歷柵格搜索巡邏車路徑，找到估價函數值最小的最優路徑，將最優路徑作為巡邏車的移動路徑；

步驟3，根據運動學模型求解巡邏車的航向角和速度：通過陀螺儀所檢測的角度、速度和加速度建立巡邏車的運動學模型；

步驟4，巡邏車速度、航向角和位置閉環控制：設計巡邏車速度、航向角和位置閉環控制雙環串聯PID，首先設計巡邏車的速度值和航向角的PID，實現速度和航向角的閉環控制，同時再接入巡邏車的位置閉環，最終實現巡邏車的位置跟蹤；

步驟5，巡邏路線上的異常情況：如果路線出現障礙物后，后臺監控軟件通過通訊將更新后的柵格地圖發送給巡邏車的控制軟件，控制軟件通過通訊中斷接收更新后的數據，并根據更新后的柵格地圖重復步驟2至步驟4。

2.根據權利要求1所述的一種高精度循跡和控制的巡邏車路徑規劃及跟蹤算法，其特征在于：步驟2中設計巡邏車路徑規劃模型的過程可以表示為：

步驟2.1，將巡邏車起點加入開放列表中，作為當前節點；

步驟2.2，查找與當前節點相鄰的節點，并將可達到的節點放入開放列表中，同時將當前節點設為這些節點的父節點；

步驟2.3，把當前節點從開放列表中移除放入封閉列表中，并在開放列表中尋找遍歷尋找估價函數值最小的節點，同時將估價函數值最小的節點放入關閉列表里，并重復步驟2.2和步驟2.3，直到將終點放入封閉列表中，則停止尋優；

評估函數f(n)具體公式為：

f(n)＝g(n)+h(n)

其中，g(n)是第n個節點到上一個節點的移動代價最佳路徑值；h(n)是選擇第n個節點后，起點到終點的最佳路徑的啟發值。

3.根據權利要求1所述的一種高精度循跡和控制的巡邏車路徑規劃及跟蹤算法，其特征在于：步驟3中根據運動學模型求解巡邏車的航向角和速度的過程可以表示為：

將巡邏車運動學模型設為：

式中，巡邏車速度的微分值、是航向角度的微分值、為路徑橫向坐標的微分值、為路徑縱向坐標的微分值、a是加速度、R是轉彎半徑、是航向角；并通過微分方程求解巡邏車的定積分值：

式中，v_k+1為巡邏車在k+1的速度值，為巡邏車在k+1的航向角，X_k+1為巡邏車在k+1的路徑橫向坐標，Y_k+1為巡邏車在k+1的路徑縱向坐標，v_k為巡邏車在k的速度值，為巡邏車在k的航向角，X_k為巡邏車在k的路徑橫向坐標，Y_k為巡邏車在k的路徑縱向坐標，t為時間變量；巡邏車在時刻k+1和時刻k之間，設行駛的距離為Δd，行駛的航向角為則可得：

式中Δt是時刻k+1和時刻k的間隔時間。

4.根據權利要求1所述的一種高精度循跡和控制的巡邏車路徑規劃及跟蹤算法，其特征在于：步驟4中巡邏車速度、航向角和位置閉環控制的過程可以表示為：

為提高巡邏車控制精度、減小系統的超調量，其中實現速度、航向角和位置的閉環控制均采用積分分離的PID控制，在算法中引入邏輯功能，調節器輸出采樣點值為：

其中，u(i)是PID的輸出，e(i)是i時刻系統誤差，e(i-1)是i-1時刻系統誤差，i為離散化的時間變量，K_p，K_l1，K_d，分別是PID算法的比例系數、積分系數和微分系數，e(j)是j時刻的系統誤差，A為PID控制系統門限，當偏差較大時，PID的積分項不起作用，偏差在門限之內時，引入積分算法，來控制巡邏車的運行。