配置空間

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

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

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

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

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

配置空間的意義：

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

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

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

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

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

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

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

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

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

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

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

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

圖的基礎(chǔ)概念

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

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

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

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

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

圖搜索基本概念

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

上圖是抽象概念里的圖

對于實際場景，我們需要人為構(gòu)造一個圖，以下是兩種簡單的例子

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

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

圖搜索算法

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

對于搜索節(jié)點，可以構(gòu)建一個搜索樹

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

從起點搜索到終點后，回溯整個搜索過程，就可以得到希望的搜索路徑

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

圖搜索算法框架

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

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

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

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

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

有兩個下面的問題需要注意：

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

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

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

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

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

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

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

圖遍歷算法：

廣度優(yōu)先搜索

深度優(yōu)先搜索

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

彈出元素，總是從隊列的頭彈出的

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

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

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

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

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

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

BFS步驟的拆分：

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

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

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

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

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

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

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

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

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

特點：每次彈出的節(jié)點是最深層級的一個節(jié)點，維護(hù)的是一個堆棧

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

DFS步驟的拆分：

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

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

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

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

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

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

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

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

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

BFS vs DFS

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

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

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

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

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