[發明專利]一種曲率連續的路徑規劃方法、系統及設備

有效

• 申請號: 202110076203.9
• 申請日: 2021-01-20
• 公開（公告）號: CN112904858B
• 公開（公告）日: 2022-04-22
• 發明（設計）人: 鄭南寧;簡志強;張崧翌;南智雄;符嘉瑋;陳仕韜
• 申請（專利權）人: 西安交通大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 西安通大專利代理有限責任公司 61200
• 代理人: 王艾華

本發明公開一種曲率連續的路徑規劃方法、系統及設備，方法包括以下步驟：獲取原始路點，生成曲率變化率連續的自然參數曲線，定義局部規劃使用的坐標架；獲取機器人在笛卡爾坐標系的位姿，基于所述位姿得到機器人在所述坐標架中的對應點；進一步解算出機器人在以所述坐標架為橫軸的曲線坐標系下的位姿；在曲線坐標系下采樣多個目標點，利用所述多個目標點和作為邊界條件解算出多條路徑的參數曲線，再將每一條路徑從曲線坐標系轉換回笛卡爾坐標系；采用損失函數對每一條路徑進行評價，損失最小的路徑作為當前最優路徑，在曲線坐標系中進行規劃仍然可以保證規劃路徑的曲率連續性，可以在順應全局導航路徑趨勢的同時，實現局部避障。

技術領域

本發明屬于自主移動機器人技術領域，具體涉及一種曲率連續的路徑規劃方法、系統及設備。

背景技術

輪式機器人的路徑規劃算法往往分為全局規劃和局部規劃兩個部分。相比于直接利用全局規劃信息進行局部規劃，將全局規劃路徑作為進行局部規劃可以適應不同道路。這種方式也稱之為曲線坐標系下的規劃。然而，當前在曲線坐標系下的路徑規劃算法無法實現規劃路徑曲率的連續，這會對機器人的行駛造成不利影響。

全局規劃路徑為機器人提供導航，它可以來源于地圖的先驗信息，也可以由A星等算法生成。使用螺線或參數曲線生成的局部路徑往往難以擬合全局路徑。為此，研究者們提出了將全局路徑作為坐標架進行局部規劃的方法。在這種方式下，機器人的位置、朝向和曲率將被轉換到以全局路徑為橫坐標的曲線坐標系下，作為局部路徑規劃的初始條件。待曲線坐標系下的局部路徑生成完畢后，再將路徑轉換到笛卡爾坐標系下，提供給控制模塊。但是，問題在于從笛卡爾坐標系到曲線坐標系的曲率轉換公式并沒有被推導出來。為此，如果放棄使用機器人當前曲率作為局部路徑規劃的初始條件，使用三次樣條進行曲線坐標系中的局部路徑生成；這種方法會使得生成的局部路徑初始曲率與機器人當前曲率不一致，導致曲率的不連續。

發明內容

為了解決現有技術中存在的問題，本發明提供一種在曲線坐標系下能夠規劃出曲率連續路徑的方法，本發明獲取從笛卡爾坐標系到曲線坐標系的位置、朝向、曲率轉換公式，以及從曲線坐標系到笛卡爾坐標系的位置、朝向、曲率轉換公式，基于所推導出的公式，包含了全局規劃和局部規劃兩個部分。全局規劃生成曲率變化率連續的路徑作為坐標架，局部規劃在該坐標架中利用二階連續參數曲線進行局部路徑生成，對每一條局部路徑的損失進行計算，損失最小的就是最終生成的路徑。

為了實現上述目的，本發明采用的技術方案是：一種曲率連續的路徑規劃方法，包括以下步驟：

獲取原始路點，生成曲率變化率連續的自然參數曲線，定義局部規劃使用的坐標架

獲取機器人在笛卡爾坐標系的位姿I_a，基于所述位姿I_a得到機器人在所述坐標架中的對應點R；根據機器人在笛卡爾坐標系的位姿I_a以及機器人在所述坐標架中的對應點R解算出機器人在以所述坐標架為橫軸的曲線坐標系下的位姿I_u；

在曲線坐標系下采樣多個目標點，利用所述多個目標點和I_u作為邊界條件解算出多條路徑的參數曲線，再將每一條路徑從曲線坐標系轉換回笛卡爾坐標系；

采用損失函數對每一條路徑進行評價，損失最小的路徑作為當前最優路徑。

原始路點來自高精地圖的道路中心線或搜索算法得到的初始路徑點。

基于原始路點，使用四次樣條進行插值生成曲率變化率連續的自然參數曲線，具體如下：建立分段函數結合滿足G³連續性條件以及s與u的關系，解算出對應的自然參數曲線再根據自然參數曲線計算出其朝向曲率κ(s)＝θ′(s)和曲率變化率σ(s)＝κ′(s)，其中，Arg為計算向量朝向的函數，得到局部規劃的坐標架