[發明專利]一種路徑的軌跡修正方法、裝置、設備和介質

在審

• 申請號: 202211234824.6
• 申請日: 2022-10-10
• 公開（公告）號: CN115639817A
• 公開（公告）日: 2023-01-24
• 發明（設計）人: 陳弓;李承遠;盧維;余冬冬;高炳舒;張力夫
• 申請（專利權）人: 浙江華睿科技股份有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02;G06Q10/047
• 代理公司: 北京同達信恒知識產權代理有限公司 11291
• 代理人: 張潔

1.一種路徑的軌跡修正方法，其特征在于，所述方法包括：

獲取輸入的待修正路徑的初始路徑點的坐標集合，進行B樣條擬合得到所述待修正路徑的每個控制點的位置；

針對所述每個控制點，根據該控制點以及該控制點之前的相鄰兩個控制點，確定該控制點與上一控制點的連線朝向的第一朝向角度值、以及上一控制點與再上一控制點的連線朝向的第二朝向角度值，并確定所述第二朝向角度值與所述第一朝向角度值的角度差值，和/或根據該控制點的位置以及預先保存的歐幾里得符號距離場ESDF地圖，確定該控制點對應的最近障礙物的距離，根據該控制點對應的角度差值和/或距離，進行角度優化和/或障礙物距離優化確定該控制點的更新后的位置；

根據所述每個控制點的更新后的位置，進行B樣條距離采樣得到所述待修正路徑軌跡修改后的每個路徑點的位置。

2.根據權利要求1所述的方法，其特征在于，所述根據該控制點的位置以及預先保存的歐幾里得符號距離場ESDF地圖，確定該控制點對應的最近障礙物的距離包括：

根據該控制點的位置、預先保存的歐幾里得符號距離場ESDF地圖，確定該控制點的位置在所述ESDF地圖中對應的第一目標體素，將所述第一目標體素對應保存的距離確定為該控制點對應的最近障礙物的距離；或，

根據該控制點的位置以及歐幾里得符號距離場ESDF地圖，在所述ESDF地圖中確定該控制點的位置對應的第一目標體素，根據預先保存的自動導向車AGV模型、以及設定數量的每個預設球體模型，將所述AGV模型的運動中心的位置放置于所述ESDF地圖的所述第一目標體素處，得到所述AGV模型上所述每個預設球體模型的每個球心位置對應的每個第二目標體素，根據所述每個第二目標體素對應保存的距離，確定最小距離為該控制點對應的最近障礙物的距離；或，

根據該控制點的位置以及歐幾里得符號距離場ESDF地圖，在所述ESDF地圖中確定該控制點的位置對應的第一目標體素，根據預先保存的自動導向車AGV模型、以及所述AGV模型的每個預設表面點，將所述AGV模型的運動中心的位置放置于所述第一目標體素處，得到所述AGV模型的所述每個預設表面點對應的每個第三目標體素點，根據所述每個第三目標體素對應保存的距離，確定最小距離為該控制點對應的最近障礙物的距離。

3.根據權利要求1所述的方法，其特征在于，所述根據該控制點對應的角度差值，進行角度優化定該控制點的更新后的位置包括：

若所述角度差值大于預設角度閾值，則確定所述角度差值與預設角度閾值的差值，將所述差值的平方值與預設權重值的乘積值確定為該控制點的目標代價值，將所述目標代價值與預設標準梯度向量的乘積向量確定為該控制點的目標梯度向量；若所述角度差值不大于所述預設角度閾值，則將預設代價值確定為該控制點的目標代價值，將預設梯度向量確定為該控制點的目標梯度向量；

將所述目標代價值和所述目標梯度向量輸入預先保存的優化器，得到輸出的該控制點進行角度優化后的位置，根據該控制點的角度優化后的位置，重復進行角度優化直到優化次數滿足優化終止條件后確定該控制點的更新后的位置。

4.根據權利要求1所述的方法，其特征在于，所述根據該控制點對應的距離，進行障礙物距離優化確定該控制點的更新后的位置包括：

若所述距離大于預設距離閾值，則確定所述距離與預設距離閾值的差值，將所述差值的平方值與預設權重值的乘積值確定為該控制點的目標代價值，將所述目標代價值與預設標準梯度向量的乘積向量確定為該控制點的目標梯度向量，若所述距離不大于所述預設距離閾值，則將預設代價值確定為該控制點的目標代價值，將預設梯度向量確定為該控制點的目標梯度向量；

將所述目標代價值和所述目標梯度向量輸入預先保存的優化器，得到輸出的該控制點進行障礙物距離優化后的位置，根據該控制點的障礙物距離優化后的位置，重復進行障礙物距離優化直到優化次數滿足優化終止條件后確定該控制點的更新后的位置。