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

有效

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

1.一種基于彎曲柱坐標系的局部平滑軌跡規劃方法，其特征在于，該方法的具體步驟為：

步驟一：根據地圖信息和無人載具目標任務要求，規劃一條平滑的全局路徑；

步驟二：以全局路徑為基線，創建彎曲柱坐標系；

步驟三：對無人載具可視空間進行環境構建，提取無人載具可視空間坐標系內的障礙物信息；

步驟四：將無人載具的運動狀態信息由全局坐標系轉換到彎曲柱坐標系中；將無人載具可視空間內的障礙物信息轉換為全局坐標系障礙物信息；

步驟五、根據全局路徑，利用代價函數和時間空間離散化算法對未來時間T內，無人載具的局部運動軌跡進行規劃；獲取多條可能的局部運動軌跡；

其中，代價函數為：

C＝k_rC_r+k_zC_z+k_θC_θ (1)

其中，C為總代價函數，k_r為徑向代價函數的比例系數，C_r為徑向代價函數，k_z為航向代價函數的比例系數，C_z為航向代價函數，k_θ為螺旋代價函數的比例系數，C_θ為螺旋代價函數；

徑向代價函數C_r為：

其中，C_t(r(t))為徑向加速度變化率的優化指標，r(T)是本次局部規劃末端時刻位置距離全局路徑的距離，r(t)是t時刻無人載具位置距離全局路徑的距離，是本次局部規劃時間指標的徑向權重系數，本次局部規劃加速度變化率指標的徑向權重系數，是本次局部規劃末端狀態指標的徑向權重系數，就是r(t)對時間的三階導的平方；

航向代價函數為：

其中，C_t(z(t))為航向加速度的變化率的優化指標，z(t)是t時刻無人載具的航向運動的路程，h(z(T),是本次局部規劃末端時刻z軸方向的代價函數；z(T)是本次局部規劃結束時刻z軸方向的值，為z(T)的一階導數，為z(T)的二階導數，是本次局部規劃加速度變化率指標的航向權重系數，是本次局部規劃時間指標的航向權重系數，是本次局部規劃末端狀態指標的航向權重系數；

螺旋代價函數為：

其中，C_t(θ(t))為螺旋加速度的變化率的優化指標，θ(0)為初始時刻的螺旋角，θ(t)為t時刻的螺旋角，是本次局部規劃加速度變化率指標的螺旋方向權重系數，是本次局部規劃末端狀態指標的螺旋方向權重系數，是本次局部規劃時間指標的螺旋方向權重系數；

獲取多條可能的局部運動軌跡的具體方法為：

利用時間空間離散化算法和徑向加速度變化率的優化指標、航向加速度變化率的優化指標和螺旋加速度變化率的優化指標求取t時刻無人載具位置距離全局路徑的距離r(t)的表達式、t時刻無人載具的航向運動的位移z(t)的表達式和t時刻的螺旋角θ(t)的表達式；

無人載具位置距離全局路徑的距離r(t)的表達式求取的具體方法為：

令：

要求C_t(r(t))最小，需要r(t)滿足所以以加速度變化率的累積為指標的變量r(t)一定滿足五次多項式關系：

r(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵ (6)

a₀、a₁、a₂、a₃、a₄、a₅均為五次多項式的系數，將初值和終值帶入得：

r(0)＝a₀ (7)

r(T)＝a₀+a₁T+a₂T²+a₃T³+a₄T⁴+a₅T⁵ (10)

獲得a₀、a₁和a₂的值；其中，r(0)是初始時刻無人載具距全局路徑的距離，是初始時刻無人載具的徑向速度，是初始時刻無人載具的徑向加速度，r(T)是本次局部規劃末端時刻無人載具的目標徑向位置，本次局部規劃末端時刻無人載具的目標徑向速度，是本次局部規劃末端時刻無人載具的目標徑向加速度；

將(7)(8)(9)帶入(10)(11)(12)得：

求解出a₃、a₄和a₅，得到r(t)的表達式；同理獲得z(t)的表達式和θ(t)的表達式，進而獲得可能的局部運動軌跡為：

其中，b₀、b₁、b₂、b₃、b₄、b₅、c₀、c₁、c₂、c₃、c₄、c₅分別為表達式z(t)的系數和表達式θ(t)的系數；

步驟六、根據步驟四轉換獲得的全局坐標系障礙物信息，從多條可能的局部運動軌跡中刪除無效的局部運動軌跡，獲取多條有效局部運動軌跡；

步驟七、計算每條有效局部運動軌跡的代價函數值，并將代價函數值最低的局部運動軌跡作為本次局部規劃的最優運動軌跡。