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

有效

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

1.一種曲率連續的路徑規劃方法，其特征在于，包括以下步驟：

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

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

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

采用損失函數對每一條路徑進行評價，損失最小的路徑作為當前最優路徑；基于機器人在笛卡爾坐標系的位姿為I_a＝(x,y,β_a,α_a)，通過牛頓迭代求解表達式：得到s_c，將s_c代入中求解得到機器人在坐標架的對應點R＝(X,Y,θ,κ,σ)；根據機器人在笛卡爾坐標系下的位姿I_a＝(x,y,β_a,α_a)和坐標架中的對應點R＝(X,Y,θ,κ,σ)，求解出機器人在曲線坐標系下的位姿I_u＝(l,r,β_u,α_u)，機器人從笛卡爾坐標系到曲線坐標系下的位姿轉換公式為：

其中，Sgn為符號函數，且Q＝-2+rκ(2-rκ)+rκ(-2+rκ)cos(2β_a-2θ)，x、y、β_a和α_a分別為笛卡爾坐標系中橫坐標、縱坐標、朝向和曲率，X、Y、θ、κ和σ分別為橫坐標、縱坐標、朝向、曲率，表示曲率變化率。

2.根據權利要求1所述的曲率連續的路徑規劃方法，其特征在于，原始路點來自高精地圖的道路中心線或搜索算法得到的初始路徑點。

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

4.根據權利要求1所述的曲率連續的路徑規劃方法，其特征在于，采樣過程中將每一條局部路徑的每一個采樣頂點從曲線坐標系轉換到笛卡爾坐標系，具體為：當前需要進行坐標轉換的定位位姿為(l,r,β_u,α_u)，坐標架中對應點為R＝(X,Y,θ,κ,σ)，采樣頂點在笛卡爾坐標系下的坐標為(x,y,β_a,α_a)，從曲線坐標系到笛卡爾坐標系的位姿轉換公式為：

其中x、y、β_a和α_a分別為笛卡爾坐標系中橫坐標、縱坐標、朝向和曲率，X、Y、θ、κ和σ分別為橫坐標、縱坐標、朝向、曲率，表示曲率變化率。

5.根據權利要求1所述的曲率連續的路徑規劃方法，其特征在于，以10Hz的頻率進行重規劃，不斷更新當前最優路徑。

6.根據權利要求1所述的曲率連續的路徑規劃方法，其特征在于，所述損失函數為：

cost＝cost_obs+cost_smo

其中，cost_obs用于衡量路徑的碰撞風險，cost_smo用于評價路徑的平滑性；cost_obs用于判斷每一條路徑是否與障礙物發生碰撞，之后利用離散高斯卷積計算碰撞路徑對于相鄰路徑的影響，每一條路徑將自己受到碰撞路徑的影響進行疊加就可以得到自己的碰撞風險損失，cost_smo的計算為路徑采樣頂點曲率平方的求和。