[發明專利]一種基于移動機器人輪廓的安全運動規劃方法和裝置

在審

• 申請號: 202110647515.0
• 申請日: 2021-06-08
• 公開（公告）號: CN113741414A
• 公開（公告）日: 2021-12-03
• 發明（設計）人: 李靜;王軍政;武慶斌;汪匯洋;汪首坤;趙江波;馬立玲;沈偉;李金倉
• 申請（專利權）人: 北京理工大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 北京理工大學專利中心 11120
• 代理人: 郭德忠

1.一種基于移動機器人輪廓的安全運動規劃方法，其特征在于，包括：

基于已有環境地圖信息、機器人定位信息進行軌跡規劃，獲得初始路徑；

考慮機器人的運動學約束、兩點邊界值約束、流形約束，基于所述初始路徑進行路徑優化，完成機器人的運動規劃；

所述流形約束包括障礙物碰撞約束條件，具體為：將機器人的輪廓表示為凸多邊形，將障礙物的輪廓表示為一個以上的凸多邊形；在任意時刻，機器人凸多邊形輪廓的每個頂點均位于障礙物凸多邊形的外部，障礙物凸多邊形的每個頂點均位于機器人凸多邊形輪廓的外部。

2.如權利要求1所述的方法，其特征在于，所述機器人凸多邊形輪廓的頂點位于障礙物凸多邊形的外部，障礙物凸多邊形的頂點位于機器人凸多邊形輪廓的外部的判斷方式為：

設頂點為p_v，凸多邊形T由頂點V₁…V_n組成；判斷頂點為p_v是否在凸多邊形T外部的方式為：以頂點p_v依次向頂點V₁…V_n做連線，n條連線組成n個三角形，表示為p_vV_nV₁、p_vV_kV_k+1，k＝1,2,…,n-1；如果n個三角形的總面積大于凸多邊形T的面積，則判定頂點p_v在凸多邊形T的外部。

3.如權利要求1所述的方法，其特征在于，所述兩點邊界值約束包括起點約束和終點約束兩部分：

起點約束表示為：

終點約束表示為：

且

其中，x_m(t)和y_m(t)表示機器人在t時刻的位置信息，v_m(t)和a_m(t)分別代表機器人在t時刻運動的速度和加速度信息；θ_m(t)為機器人在t時刻的偏航角信息；δ_m(t)和ω_m(t)分別表示機器人輪子在t時刻的轉向角和轉向角速度；t_f表示停止時刻；(x_m0,y_m0)表示機器人在初始時刻的位置坐標信息，θ_m0代表機器人在初始時刻的偏航角方向信息，v_m0和δ_m0分別為機器人在初始時刻的速度和轉向角信息；(x_mf,y_mf)代表機器人停止時刻的位置坐標信息，且θ_mf表示機器人停止時刻的偏航角方向信息。

4.如權利要求1所述的方法，其特征在于，所述流形約束進一步包括運動能力極值約束：

其中，v_m(t)和a_m(t)分別代表機器人在t時刻運動的速度和加速度信息，δ_m(t)和ω_m(t)分別表示機器人輪子在t時刻的轉向角和轉向角速度；v_max為機器人在整個運動過程中運動速度的最大值，δ_max為機器人在整個運動過程中輪子的轉向角所能達到的最大值，a_max和ω_max分別表示機器人在整個運動過程中加速度和輪子轉向角速度的最大值。

5.如權利要求1所述的方法，其特征在于，所述基于已有環境地圖信息、機器人定位信息進行軌跡規劃，獲得初始路徑為：在已有環境地圖信息、機器人定位信息的基礎上，通過路徑回溯算法規劃出一條無碰撞的連接起點與終點的幾何路徑；然后參考環境中的動態障礙物信息，采用速度決策方法規劃出機器人以何種速度沿著幾何路徑行駛，從而完成前端的軌跡規劃，獲得初始路徑。

6.如權利要求1所述的方法，其特征在于，所述路徑優化過程中，采用軌跡對應通過時間、能量消耗、軌跡平滑度的加權和衡量路徑質量，并作為優化的目標函數。