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

在審

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

本發明提供一種高精度循跡和控制的巡邏車路徑規劃及跟蹤算法。該方法包括以下步驟：步驟1，柵格化巡邏車使用地的地理；步驟2，設計巡邏車路徑規劃模型；步驟3，根據運動學模型求解巡邏車的航向角和速度；步驟4，巡邏車速度、航向角和位置閉環控制；步驟5，處理巡邏路線上的異常情況，更新尋優路徑。本發明實現了高速及高精度的巡邏車路軌規劃尋優算法，提高了巡邏車路徑尋優的效率，同時在巡邏車的控制問題中，提出了雙環串聯PID閉環的控制系統，有效地克服巡邏車運動時的非線性因素和系統摩擦。

技術領域

本發明涉及巡邏車循跡領域，特別是涉及一種高精度循跡和控制的巡邏車路徑規劃及跟蹤算法。

背景技術

隨著社會不斷發展，各領域的技術研究不斷深入，各種智能代理技術逐漸融入人們的日常工作和生活，幫助人們從枯燥的重復勞動中解脫出來，提高了生產效率，降低了生產成本。

巡邏車作為目前建筑、安防行業運用最廣泛的機器人，其在安全監控、巡邏監測等方面有巨大的價值。巡邏車路徑規劃的目標是利用合適的路徑規劃方法尋找從起點到終點的無碰撞的最優路徑，在可控的結構化工廠環境中，巡邏車具有一定的自主性，能夠感知周圍環境，檢測障礙物，并規劃一條無碰撞的路徑導航到目的地。但在礦山、巖洞、施工工地等復雜的動態施工空間環境中，對巡邏車的應用要實現完全自主，尚未得到充分解決，并且在特定的平面布置圖上規劃巡邏車的路徑需要有關起點和終點(即現場通道和目標工作區)、靜態障礙物(如墻、柱)和動態危險區域(如材料、工人和設備工作區)的信息.還要實現幾何空間的定位及距離測算。因此，有必要根據在不同時間節點處于現場不同位置進行的施工活動、工作空間分布和變化實現機器人系統全局路徑規劃和路徑跟蹤控制。

發明內容

為解決上述問題，本發明在實時采集的行駛場地數據基礎上，提出了一種高精度循跡和控制的巡邏車路徑規劃及跟蹤算法。為尋找巡邏車的最優路徑，設計了一種巡邏車路徑規劃模型，同時為了提高巡邏車運行時的準確率，提出了巡邏車速度、航向角和位置閉環控制，并通過實時更新最優路徑，來處理異常出現的障礙物。本發明提供一種高精度循跡和控制的巡邏車路徑規劃及跟蹤算法，具體步驟如下，其特征在于：

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

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

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

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

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

進一步，步驟2中設計巡邏車路徑規劃模型的過程可以表示為：

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

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

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