[發(fā)明專利]一種基于彎曲柱坐標系的局部平滑軌跡規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 202010410092.6
• 申請日: 2020-05-14
• 公開（公告）號: CN111552296B
• 公開（公告）日: 2021-03-26
• 發(fā)明（設計）人: 高會軍;李湛;宋罘林;于興虎
• 申請（專利權）人: 寧波智能裝備研究院有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 哈爾濱市松花江專利商標事務所 23109
• 代理人: 張利明

一種基于彎曲柱坐標系的局部平滑軌跡規(guī)劃方法，無人載具軌跡規(guī)劃技術領域。本發(fā)明解決了現(xiàn)有三維空間內(nèi)無人載具局部路徑規(guī)劃方法軌跡連續(xù)性和平滑性差，且軌跡約束難的問題。本發(fā)明根據(jù)地圖信息和無人載具目標任務要求，規(guī)劃一條平滑的全局路徑；以全局路徑為基線，創(chuàng)建彎曲柱坐標系；將無人載具的運動狀態(tài)信息由全局坐標系轉換到彎曲柱坐標系中；根據(jù)全局路徑，利用代價函數(shù)和時間空間離散化對未來時間T內(nèi)，無人載具的局部運動軌跡進行規(guī)劃；刪除與障礙物碰撞的和超出無人載具動力約束范圍的局部運動軌跡，將代價函數(shù)值最低的局部運動軌跡作為局部規(guī)劃的最優(yōu)運動軌跡。本發(fā)明適用于無人載具的局部運動軌跡的規(guī)劃。

技術領域

本發(fā)明屬于無人載具軌跡規(guī)劃技術領域。

背景技術

近年來，三維空間內(nèi)無人載具，例如，小型無人潛艇、無人機等由于其模型簡單、制作方便、響應迅速、機動性好、成本低廉等優(yōu)點，吸引力一大批國內(nèi)外研究者的關注。然而在復雜的三維空間中，實時規(guī)劃出能成功避障、軌跡平滑且運動狀態(tài)不突變的局部路徑一直是實現(xiàn)無人載具高動態(tài)運行的主要研究課題之一。保證三維空間內(nèi)既要快速平穩(wěn)的通過事先未知障礙的長直區(qū)域，又要成功準確的避開障礙到達目標點仍然是一件十分具有挑戰(zhàn)性的工作。本工作聚焦于在已知全局路徑的情況下獲得最優(yōu)的局部路徑的方法，本質(zhì)為最優(yōu)化問題。

三維空間內(nèi)無人載具任務層面的規(guī)劃與控制往往可以分成三個層次：全局路徑規(guī)劃、局部路徑規(guī)劃、局部路徑跟隨。三個層次由高到低、由抽象到具體：全局路徑規(guī)劃(Global path planning)在整個任務規(guī)劃的最高層，需要在無人載具運行之前根據(jù)現(xiàn)有的任務以及地圖等約束信息獲取到一條由起點到終點的平滑的路徑(path)；局部路徑規(guī)劃(Local trajectory planning)在整個任務規(guī)劃的第二層，局部路徑規(guī)劃在已經(jīng)獲得的全局路徑的基礎上，再結合三維空間內(nèi)無人載具上攜帶的傳感器實時觀測到的周圍環(huán)境的信息，實時規(guī)劃出一條條符合當前環(huán)境約束以及無人載具動力學約束的軌跡(trajectory)；局部路徑跟隨(Path tracking)在無人載具任務規(guī)劃與控制的最下層，也是最具體的一層，無人載具要根據(jù)自己當前的狀態(tài)以及上一層獲取到的局部軌跡，使用基于模型或無模型的控制率，實現(xiàn)以較低的偏差跟隨局部路徑。在整個任務規(guī)劃與控制的過程中，以上任何一個層次都是現(xiàn)今的研究重點，而局部路徑規(guī)劃位于其中承上啟下的位置，更是其中的難點，既要保證規(guī)劃出的局部軌跡滿足某種程度的最優(yōu)(速度、曲率、平滑等)，又要保證路徑的可行性(避障、滿足動力學等)。

目前局部路徑規(guī)劃的方法可分為基于自主學習的和非自主學習類，其中基于深度學習的端到端的規(guī)劃與控制方法以及基于強化學習的算法雖然近年來在理論上得到了長足的發(fā)展，但無論是安全性、可靠性還是穩(wěn)定性都有很大的問題，對于未知的環(huán)境更是根本無法使用；非自主學習類算法中，可分為基于搜索的規(guī)劃算法和基于采樣的規(guī)劃算法。基于搜索的規(guī)劃算法主要的代表者是A*算法和Dijkstra算法，這兩種算法在很多場合都有很好的應用，尤其在游戲和汽車導航等領域，但是它們規(guī)劃時間長、消耗算力大，并不能很好的應用在障礙位置未知的區(qū)域進行高動態(tài)運行的無人載具上；基于采樣的規(guī)劃算法主要的代表者是RRT算法和PRM算法，但是三維空間中采樣的隨機性更大，而且得到的路徑不但不能保證最優(yōu)，而且會出現(xiàn)很多毛刺和抖動，超出無人載具的模型約束和動力學范圍。

為解決上述問題，出現(xiàn)了一類更加適合三維空間內(nèi)無人載具路徑規(guī)劃的特殊曲線拼接的方法：基于平滑連接直線和圓弧的路徑規(guī)劃算法能根據(jù)無人載具的初始狀態(tài)和目標配置輕易的得到一條平順的路徑，但是得到的路徑靈活性差，直線和圓弧的連接處三維空間內(nèi)無人載具的向心加速度有突變，對后面無人載具的路徑跟隨要求更高；基于Bezier曲線的路徑規(guī)劃算法得到的路徑更加“智能化”，它的四個控制點均可以編輯，但是Bezier曲線拼接時，滿足幾何連續(xù)性條件是比較困難的，難以對其進行進一步的優(yōu)化；基于B樣條曲線的路徑規(guī)劃算法能得到平滑拼接的路徑，但是B樣條曲線的曲率表示困難，不容易保證、三維空間內(nèi)無人載具的曲率約束條件。

發(fā)明內(nèi)容