[發(fā)明專利]一種移動型機器人安全運動軌跡的實時規(guī)劃方法

在審

• 申請?zhí)枺?/td>: 202011288150.9
• 申請日: 2020-11-17
• 公開（公告）號: CN112432648A
• 公開（公告）日: 2021-03-02
• 發(fā)明（設(shè)計）人: 勞健濤;成慧
• 申請（專利權(quán)）人: 中山大學(xué)
• 主分類號: G01C21/20
• 分類號: G01C21/20;G06Q10/04
• 代理公司: 廣州粵高專利商標代理有限公司 44102
• 代理人: 王曉玲

1.一種移動型機器人安全運動軌跡的實時規(guī)劃方法，其特征在于，包括以下步驟：

S1.拓撲路徑搜索，包括地圖柵格化、基于采樣的路徑搜索和拓撲路徑選取三個步驟，得到優(yōu)化后能量最小的路徑作為整個拓撲路徑搜索過程的最終軌跡；

S2.根據(jù)步驟S1選取的最終軌跡作為初始軌跡進行局部梯度信息的構(gòu)建；

S3.進行軌跡優(yōu)化，基于軟約束的優(yōu)化方法，考慮軌跡的平滑性、安全性和動力學(xué)約束三個指標，通過加權(quán)得到整體軌跡的優(yōu)化方程。

2.根據(jù)權(quán)利要求1所述的移動型機器人安全運動軌跡的實時規(guī)劃方法，其特征在于，所述的地圖柵格化是指使用適合的柵格大小對全局3D地圖進行柵格化，即將連續(xù)的感知三維空間轉(zhuǎn)化成離散的柵格地圖，其最小單位為一個柵格大小，使得生成的柵格地圖在僅丟失少量精度的情況下能夠大大減少地圖信息的計算量。

3.根據(jù)權(quán)利要求1所述的移動型機器人安全運動軌跡的實時規(guī)劃方法，其特征在于，所述的基于采樣的路徑搜索算法是指根據(jù)機載計算平臺的負載大小確定最大承受的采樣點數(shù)量，接著使用這個最大采樣點數(shù)量對柵格地圖進行采樣，每個采樣點都會跟與之相鄰的采樣點進行相連；其中，兩個采樣點之間的連線不能穿過障礙物，否則該兩個采樣點被視為不可見，即使兩點之間的距離最短，也不能相連；通過上述操作，最終得到多條由機器人當前位置到目標導(dǎo)航點的路徑。

4.根據(jù)權(quán)利要求3所述的移動型機器人安全運動軌跡的實時規(guī)劃方法，其特征在于，拓撲路徑選取是指對多條從機器人當前位置指向目標導(dǎo)航點的路徑進行重采樣，兩兩路徑的采樣點之間沒有存在障礙物，即表示這兩條路徑是拓撲相等的，則刪除相對來說更長的路徑，通過這一步操作，即可得到多條拓撲不等的路徑；，通過對多條拓撲不等的路徑分別進行均勻B樣條優(yōu)化，以得到平滑且連續(xù)的軌跡。

5.根據(jù)權(quán)利要求4所述的移動型機器人安全運動軌跡的實時規(guī)劃方法，其特征在于，所述的均勻B樣條優(yōu)化具體包括以下步驟：

對初始軌跡進行N個點的均勻重采樣，可以得到軌跡的控制點序列為{Q₀，Q₁，Q₂，...，Q_N-2，Q_N-1}，其中，每一個控制點Q_i均為三維空間中的一個位置點；

對軌跡進行K次均勻B樣條優(yōu)化，則有N+K個節(jié)點向量{u₀，u₁，u₂，...，u_N+K-2，u_N+K-1}，其中每一個節(jié)點的前后間隔相同，即u₀-u₁＝u₁-u₂＝u₂-u₃＝...＝u_N+K-2-u_N+K-1；得到優(yōu)化軌跡的數(shù)學(xué)表達式如下：

其中，N_i，k(u)為k次均勻B樣條優(yōu)化的基函數(shù)，其公式定義為：

選擇一條優(yōu)化后能量最小的路徑作為整個拓撲路徑搜索過程的最終軌跡，也是下一步局部梯度信息構(gòu)建過程和軌跡優(yōu)化過程的初始軌跡。

6.根據(jù)權(quán)利要求5所述的移動型機器人安全運動軌跡的實時規(guī)劃方法，其特征在于，局部梯度信息的構(gòu)建的過程包括：

首先通過對初始軌跡進行相同步長的重采樣，得到軌跡上一系列均勻的采樣點；

根據(jù)無人機大小以及飛行安全距離，預(yù)先設(shè)置一個距離閾值；

接著依次遍歷整個軌跡上的采樣點，確定與其距離最近的障礙物表面的點，若其距離小于所設(shè)置的距離閾值，則保留兩個采樣點以及其距離大小；

當軌跡上的采樣點遍歷結(jié)束，可以得到一個包含多個采樣點對以及其距離大小的集合，是下一步中對軌跡進行進一步優(yōu)化所用到的局部梯度信息。