配置空間

機(jī)器人規(guī)劃的配置空間概念：一個(gè)空間包含所有機(jī)器人自由度的機(jī)器人配置，描述為C-space

機(jī)器人配置：表示對(duì)機(jī)器人上面所以點(diǎn)的位置的描述

機(jī)器人自由度：規(guī)劃的時(shí)候用最少的坐標(biāo)數(shù)量去表示機(jī)器人配置，例如無人機(jī)規(guī)劃，在微分平坦中進(jìn)行規(guī)劃則是xyzyaw四個(gè)變量，所以對(duì)于無人機(jī)軌跡規(guī)劃來說有四個(gè)自由度。

機(jī)器人配置空間：一個(gè)空間包含所有機(jī)器人自由度的機(jī)器人配置，描述為C-space

任何可能的機(jī)器人的位姿在C-space中描述為一個(gè)點(diǎn)

配置空間的意義：

在工作空間中進(jìn)行規(guī)劃,機(jī)器人有不同的形狀和大小，比如有圓形的或方形的碰撞檢測(cè)需要知道機(jī)器人的外形，然后再做檢測(cè)，這樣是費(fèi)時(shí)費(fèi)力的。

在配置空間中做規(guī)劃

機(jī)器人在C-space中表示一個(gè)點(diǎn)，例如位置就是一個(gè)點(diǎn)屬于R3一個(gè)位姿屬于SO(3)

在配置空間中，機(jī)器人表示成了一個(gè)點(diǎn)，那么在配置空間中，障礙物也需要特殊的處理，把工作空間中的障礙物變成配置空間中的障礙物，被稱為 配置空間障礙物或者C-obstacle這個(gè)工作是在運(yùn)動(dòng)規(guī)劃前完成的，一次完成的工作。

障礙物安照機(jī)器人尺寸進(jìn)行膨脹，上面機(jī)器人被設(shè)置成了一個(gè)點(diǎn)，只要點(diǎn)在障礙物外面，就不會(huì)發(fā)生碰撞

C-space是C-obstacle 和C-free組成的

經(jīng)過配置空間的處理，路徑規(guī)劃變成了在C-free 中找到起點(diǎn)和終點(diǎn)的路徑尋找。

在工作空間中，機(jī)器人有尺寸有形狀，對(duì)于運(yùn)動(dòng)規(guī)劃會(huì)帶來困難，在配置空間中，機(jī)器人用一個(gè)點(diǎn)來描述，方便做運(yùn)動(dòng)規(guī)劃

經(jīng)過上述配置空間的操作，碰撞檢測(cè)就進(jìn)行了簡(jiǎn)化，這就是配置空間的意義。

機(jī)器人被看做是一個(gè)球體，半徑為r

對(duì)障礙物安照半徑r進(jìn)行膨脹，藍(lán)色就是膨脹后的障礙物，然后就可以進(jìn)行路徑規(guī)劃和生成。

圖和圖搜索算法的基本概念

圖的基礎(chǔ)概念

圖是有節(jié)點(diǎn)和邊的一種表達(dá)方式

各節(jié)點(diǎn)由邊連起來

邊可以是有向的，也可以無向的

邊也可以有權(quán)重，如果沒有特殊說明，可以認(rèn)為權(quán)重是一樣的。

下面則是有權(quán)重的

圖搜索基本概念

對(duì)于任何一個(gè)圖搜索算法，首先要構(gòu)造一個(gè)圖

上圖是抽象概念里的圖

對(duì)于實(shí)際場(chǎng)景，我們需要人為構(gòu)造一個(gè)圖，以下是兩種簡(jiǎn)單的例子

柵格地圖的路徑規(guī)劃，里面的節(jié)點(diǎn)和相鄰的節(jié)點(diǎn)是具有連接關(guān)系的，所以本身就是一個(gè)圖了

基于采樣的，沒有天然的節(jié)點(diǎn)關(guān)系，需要人為構(gòu)造一個(gè)圖在里面，例如上面就是通過算法構(gòu)造的有節(jié)點(diǎn)和邊組成的圖

圖搜索算法

搜索總是從起始狀態(tài)Xs開始,到Xg結(jié)束

對(duì)于搜索節(jié)點(diǎn)，可以構(gòu)建一個(gè)搜索樹

右邊和左邊是等價(jià)的，只是寫成了樹狀的結(jié)構(gòu)，這樣看彼此關(guān)系更加清晰點(diǎn)

從起點(diǎn)搜索到終點(diǎn)后，回溯整個(gè)搜索過程，就可以得到希望的搜索路徑

對(duì)于實(shí)際機(jī)器人來說，構(gòu)建整個(gè)空間的搜索樹，代價(jià)很大，所以需要盡可能快，但是不失搜索路徑的算法。

圖搜索算法框架

所有的圖搜索算法都是按照下面的框架進(jìn)行的:

1、維護(hù)一個(gè)容器，裝載將來有可能訪問的一個(gè)節(jié)點(diǎn)

2、容器初始化為空，放入的第一個(gè)節(jié)點(diǎn)就是起始狀態(tài)Xs

3、循環(huán)：根據(jù)預(yù)先定義的一個(gè)指標(biāo)或者目的，從容器中彈出一個(gè)節(jié)點(diǎn) ，稱之為訪問一個(gè)節(jié)點(diǎn)，獲取彈出節(jié)點(diǎn)所有的鄰居節(jié)點(diǎn)—擴(kuò)展，將這些鄰居節(jié)點(diǎn)裝入容器

4、結(jié)束循環(huán)：訪問到了結(jié)束狀態(tài)，或者自定義的一個(gè)指標(biāo)，結(jié)束循環(huán)

有兩個(gè)下面的問題需要注意：

什么時(shí)候結(jié)束循環(huán)？

一種可能就是容器空了，這代表沒有了我們將來要訪問的節(jié)點(diǎn)了，遍歷完了所有節(jié)點(diǎn)

搜索到了結(jié)束節(jié)點(diǎn)

如果這個(gè)圖本身是有回環(huán)的呢？

為了在圖搜索中避免形成回環(huán)，永遠(yuǎn)走不出去，需要再維護(hù)一個(gè)新的容器，該容器裝載著已經(jīng)被訪問過的節(jié)點(diǎn)，被訪問過的節(jié)點(diǎn)不能再次被訪問

圖搜索優(yōu)化的方向就是：

按照什么規(guī)則去訪問節(jié)點(diǎn)，按照什么規(guī)則彈出節(jié)點(diǎn)，使我們盡可能快的找到終止節(jié)點(diǎn)。

圖遍歷算法：

廣度優(yōu)先搜索

深度優(yōu)先搜索

廣度優(yōu)先搜索遵循先進(jìn)先出的原則，維護(hù)的是一個(gè)隊(duì)列

彈出元素，總是從隊(duì)列的頭彈出的

深度優(yōu)先搜索遵循的是后進(jìn)先出的原則，維護(hù)的是一個(gè)堆棧

彈出從最上面彈出，最后進(jìn)入容器的，最先被彈出來

廣度優(yōu)先搜索（BFS）

特點(diǎn)：每次彈出層級(jí)最淺的一個(gè)節(jié)點(diǎn)，維護(hù)的是一個(gè)隊(duì)列的結(jié)構(gòu)

所以搜索過程是一層一層進(jìn)行的

最直觀的理解就是一層一層的進(jìn)行

BFS步驟的拆分：

這樣的一個(gè)圖結(jié)構(gòu)，BFS的步驟是下面這樣的

初始化容器是空的，首先放入開始節(jié)點(diǎn)S

彈出（訪問）S節(jié)點(diǎn)，容器就再次變?yōu)榭盏?/p>

對(duì)S進(jìn)行擴(kuò)展，從圖上上可以看出，S可以擴(kuò)展的是dep

安照定義的規(guī)則，將擴(kuò)展的節(jié)點(diǎn)以規(guī)則順序放入容器中

與DFS區(qū)別就是，從下面彈出節(jié)點(diǎn)，先彈出p節(jié)點(diǎn)

然后再進(jìn)行循環(huán)

最終找到終止節(jié)點(diǎn)，結(jié)束循環(huán)，完成圖搜索。

深度優(yōu)先搜索（DFS）

特點(diǎn)：每次彈出的節(jié)點(diǎn)是最深層級(jí)的一個(gè)節(jié)點(diǎn)，維護(hù)的是一個(gè)堆棧

直觀的理解就是沒次把一個(gè)分支走到底

DFS步驟的拆分：

注意—維護(hù)的是一個(gè)棧的結(jié)構(gòu)

這樣的一個(gè)圖結(jié)構(gòu)，DFS的步驟是下面這樣的

初始化容器是空的，首先放入開始節(jié)點(diǎn)S

彈出（訪問）S節(jié)點(diǎn)，容器就再次變?yōu)榭盏?/p>

對(duì)S進(jìn)行擴(kuò)展，從圖上上可以看出，S可以擴(kuò)展的是dep

安照定義的規(guī)則，將擴(kuò)展的節(jié)點(diǎn)以規(guī)則順序放入容器中

然后深度優(yōu)先搜索是彈出容器中的，后入的節(jié)點(diǎn)（層級(jí)最深的），即d節(jié)點(diǎn)

然后再擴(kuò)展d節(jié)點(diǎn)，將擴(kuò)展節(jié)點(diǎn)放入容器，再彈出該彈出的節(jié)點(diǎn)，再擴(kuò)展，再放入的循環(huán)操作

最終找到終止節(jié)點(diǎn)，結(jié)束循環(huán)，完成圖搜索。

BFS vs DFS

從最終的遍歷結(jié)果圖中，可以看到兩者的區(qū)別

BFS是從開始節(jié)點(diǎn)一層一層的去向外擴(kuò)展，直到擴(kuò)展到了目標(biāo)節(jié)點(diǎn)，然后回溯去找到最短路徑。

DFS是從開始節(jié)點(diǎn)一條路走到頭，去找到目標(biāo)節(jié)點(diǎn)。

由結(jié)果來看，BFS 找到的路徑是最短的

所以對(duì)應(yīng)移動(dòng)機(jī)器人，在做路徑規(guī)劃找最短路徑時(shí)，做好的方法就是BFS。