[發(fā)明專利]局部路徑規(guī)劃方法、裝置

在審

• 申請?zhí)枺?/td>: 202310190138.1
• 申請日: 2023-02-24
• 公開（公告）號: CN116430846A
• 公開（公告）日: 2023-07-14
• 發(fā)明（設計）人: 岳川元;李建朋;蔣亞西;金夢磊;李成杰
• 申請（專利權）人: 浙江安吉智電控股有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 北京維正專利代理有限公司 11508
• 代理人: 趙萬凱

1.一種局部路徑規(guī)劃方法，其特征在于，包括：

基于全局代價地圖、機器人在全局代價地圖中的當前位置和目標位置，獲取機器人當前位置至目標位置的第一全局路徑；

基于所述第一全局路徑和機器人所處所述當前位置，將機器人朝所述目標位置移動指定距離的第二全局路徑，轉換為以所述當前位置為機器人坐標系原點的參考路徑點集合；

基于所述機器人在所述當前位置的速度、航向角和所述參考路徑點集合得到動態(tài)可行軌跡簇；所述動態(tài)可行軌跡簇包含至少一條可行軌跡；

利用代價函數從所述動態(tài)可行軌跡簇中選出最優(yōu)的局部路徑。

2.根據權利要求1所述的方法，其特征在于，所述基于所述第一全局路徑和機器人所處所述當前位置，將機器人朝所述目標位置移動的指定距離的第二全局路徑，轉換為以所述當前位置為機器人坐標系原點的參考路徑點集合，包括：

對所述第一全局路徑平滑處理后，再按固定間隔重新采樣，生成新平滑軌跡點集合；

對機器人所處當前位置沿所述新平滑軌跡點集合移動指定距離的第二全局路徑進行坐標轉換，獲得機器人坐標系下的以所述當前位置為機器人坐標系原點的參考路徑點集合。

3.根據權利要求2所述的方法，其特征在于，所述坐標轉換，包括：

以機器人所述當前位置為機器人坐標系原點，以機器人前進方向為機器人坐標系的X軸，以機器人前進方向的左側為Y軸；

將所述第二全局路徑包含的所有新平滑軌跡點，分別與所述當前位置的坐標求差，得到偏差向量集合；

對偏差向量集合包含的每個偏差向量按公式進行轉換，得到機器人坐標系下的以所述當前位置為機器人坐標系原點的參考路徑點集合；所述公式，包括：

x_local＝delta_x*cosθ+delta_y*sinθ，

y_local＝delta_y*cosθ-delta_x*sinθ，

delta_x和delta_y為偏差向量的坐標；θ為機器人在全局代價地圖中的航向角；x_local和y_local為機器人坐標系下的當前位置的機器人坐標。

4.根據權利要求1所述的方法，其特征在于，所述基于所述機器人在所述當前位置的速度、航向角和所述參考路徑點集合得到動態(tài)可行軌跡簇，包括：

確定機器人速度v的最大值與機器人橫擺角速度w的最大值；

以所述當前位置的速度為第一起點，速度v的最大值為上限，將速度覆蓋范圍采樣獲得m個速度；

以所述當前位置的橫擺角速度為第二起點，橫擺角速度w的最大值為上限，將橫擺角速度覆蓋范圍采樣獲得n個橫擺角速度；

利用m個速度中的第一個速度，依次匹配所有n個橫擺角速度組成n對(v,w)；結合采樣周期計算指定時間窗口內的機器人的運動軌跡，生成n組軌跡點集合；

依次遍歷m個速度，利用上一步的生成n組軌跡點集合的方法，產生m×n組動態(tài)可行軌跡點集合；將所述m×n組動態(tài)可行軌跡點集合，作為動態(tài)可行軌跡簇。

5.根據權利要求1所述的方法，其特征在于，所述利用代價函數從所述動態(tài)可行軌跡簇中選出最優(yōu)的局部路徑，包括：

利用速度代價函數、終點接近代價函數、障礙物碰撞代價、行駛方向代價函數、曲率代價函數和全局路徑保持代價函數的結合，從所述動態(tài)可行軌跡簇中選擇代價值最小的軌跡，作為最優(yōu)的局部路徑。

6.一種局部路徑規(guī)劃裝置，其特征在于，包括：

全局路徑模塊：用于基于全局代價地圖、機器人在全局代價地圖中的當前位置和目標位置，獲取機器人當前位置至目標位置的第一全局路徑；

轉換模塊：用于基于所述第一全局路徑和機器人所處所述當前位置，將機器人朝所述目標位置移動指定距離的第二全局路徑，轉換為以所述當前位置為機器人坐標系原點的參考路徑點集合；

生成模塊：用于基于所述機器人在所述當前位置的速度、航向角和所述參考路徑點集合得到動態(tài)可行軌跡簇；所述動態(tài)可行軌跡簇包含至少一條可行軌跡；

選擇模塊：用于利用代價函數從所述動態(tài)可行軌跡簇中選出最優(yōu)的局部路徑。