路徑規(guī)劃

一、基于搜索的路徑規(guī)劃

• Dijkstra

柵格地圖：對障礙物進(jìn)行膨脹處理，對機器人看作質(zhì)點。

https://github.com/AtsushiSakai/PythonRobotics

代碼解讀：建立柵格地圖、障礙物、起終點、畫圖；柵格大小、轉(zhuǎn)彎半徑、障礙物位置；

初始化地圖、初始化障礙物grip、膨脹障礙物；機器人運動方式motion，節(jié)點node，open_set、close_set、calc_index、

算法復(fù)雜度：哈希表、優(yōu)先級隊列；

• A*

算法和dijkstra差不多，加上啟發(fā)式算法，直接導(dǎo)向終點，無需搜索更大的范圍，比dikstra算法更優(yōu)

二、基于采樣的路徑規(guī)劃

• RRT

原理：快速擴(kuò)展隨機樹算法（Rapidly Exploring Random Tree）

代碼講解：設(shè)計障礙物、采樣距離、采樣步長、迭代次數(shù)、判斷是否碰撞、是否靠近終點

總結(jié)：找到路徑快、路徑較差

• RRT*

目標(biāo)：找到一條更優(yōu)的路徑

方法：找出更短的父節(jié)點

總結(jié)：速度慢

• Inform RRT*

目標(biāo)：速度更快

方法：橢圓在不斷縮小，限制采樣范圍

代碼：

• 概率路線圖算法PRM（Probabilistic Roadmap）

構(gòu)建概率路線圖

在圖上尋找路徑

總結(jié)：兩階段，速度慢，不能找到最優(yōu)路徑，

三、基于智能算法的路徑規(guī)劃

• 遺傳算法

原理：選擇、交叉、變異

種群初始化

選擇：適應(yīng)度函數(shù)（距離的倒數(shù)、角度函數(shù)）、概率輪盤賭、

交叉：交換交叉點路徑

變異：隨機選擇路徑兩個柵格

代碼：matlab

main

產(chǎn)生種群

遺傳變異

generate_continuous_path

cal_path_value計算距離，適應(yīng)度函數(shù)一部分

cal_path_smooth計算角度，適應(yīng)度函數(shù)一部分

selection輪盤賭選擇

crossover交叉

mutation變異

• 蟻群算法

和遺傳算法差不多

軌跡優(yōu)化

一、軌跡表示方法

• 多項式曲線（論文）

上述公式很重要

優(yōu)化目標(biāo)：上述最小，論文提出該觀點

公式推導(dǎo)：

建立約束s.t.：位置約束、速度約束、加速度約束、連續(xù)性約束；二次規(guī)劃問題；

代碼講解：等式構(gòu)建

不等式構(gòu)建：安全飛行走廊

閉式求解（論文）

總結(jié)：可以平滑曲線、但不會考慮障礙物信息；

無約束優(yōu)化問題、

• 貝塞爾曲線

二、軌跡優(yōu)化目標(biāo)

• mininum snap
• 軌跡長度

三、軌跡約束方法

• 軟約束

論文: 梯度下降、目標(biāo)函數(shù)求取最小值

代碼：ros仿真代碼學(xué)習(xí)

github.com/HKUST.Aerial.Robotics/grad_traj_optimization

總結(jié)：基于軟約束的軌跡優(yōu)化方法，當(dāng)目標(biāo)函數(shù)比較復(fù)雜時，可能會導(dǎo)致軌跡和障礙物產(chǎn)生碰撞

• 硬約束

貝塞爾曲線