“優(yōu)化”是生活中經(jīng)常使用的詞：開車時希望能在安全的前提下以最短時間到達(dá)目的地；雙11做功課考慮各種優(yōu)惠活動，希望獲得最大優(yōu)惠；超市進(jìn)貨時需要考慮動銷存，最大化提高物品周轉(zhuǎn)效率。 這些問題都是“最優(yōu)化問題”，也是數(shù)學(xué)建模中的典型問題，是各大數(shù)學(xué)建模比賽里的?？汀?nbsp; 優(yōu)化題型有三要素：決策變量、目標(biāo)函數(shù)、約束條件。 （1）決策變量：是決策者可以控制的因素，例如根據(jù)不同的實際問題，決策變量可以選為產(chǎn)品的產(chǎn)量、物資的運(yùn)量及工作的天數(shù)等。 （2） 目標(biāo)函數(shù)：是以函數(shù)形式來表示決策者追求的目標(biāo)。例如目標(biāo)可以是利潤最大或成本最小等。 （3） 約束條件：是決策變量需要滿足的限定條件。 歷年國賽優(yōu)化問題： 優(yōu)化問題的出發(fā)點(diǎn)是系統(tǒng)思維，其基本思路是在一定的約束條件下，保證各方面資源的合理分配， 最大限度地提升系統(tǒng)某一性能或系統(tǒng)整體性能，最終實現(xiàn)最理想結(jié)果。對于這類問題，想要建立并求解數(shù)學(xué)模型，可以參考以下思路： （1）明確目標(biāo)，分析問題背景，確定約束條件，搜集全面的客觀數(shù)據(jù)和信息。 （2）建立數(shù)學(xué)模型，構(gòu)建變量之間的數(shù)學(xué)關(guān)系，設(shè)立目標(biāo)函數(shù)。 （3）分析數(shù)學(xué)模型，綜合選擇最適合該模型的優(yōu)化方法。 （4）求解模型，通常借助計算機(jī)和數(shù)學(xué)分析軟件完成。 （5）對最優(yōu)解進(jìn)行檢驗和實施。 PS.北太天元內(nèi)已有優(yōu)化工具箱optimization，可以調(diào)用工具箱解決優(yōu)化類問題。 下面給大家分享幾種數(shù)學(xué)建模中常用優(yōu)化算法： 1、線性規(guī)劃 在人們的生產(chǎn)實踐中，經(jīng)常會遇到如何利用現(xiàn)有資源來安排生產(chǎn)，以取得最大經(jīng)濟(jì)效益的問題。此類問題構(gòu)成了運(yùn)籌學(xué)的一個重要分支—數(shù)學(xué)規(guī)劃，而線性規(guī)劃(Linear Programming 簡記 LP)則是數(shù)學(xué)規(guī)劃的一個重要分支。 1.1 用北太天元求解線性規(guī)劃問題 北太天元內(nèi)已有優(yōu)化工具箱optimization，其中的linprog等相關(guān)函數(shù)可用于求解線性規(guī)劃問題。 1.2 線性規(guī)劃特點(diǎn) 優(yōu)點(diǎn)： （1）作為經(jīng)營管理決策中的數(shù)學(xué)手段，在現(xiàn)代決策中的應(yīng)用非常廣泛。 （2）有統(tǒng)一算法，任何線性規(guī)劃問題都能求解，解決多變量最優(yōu)決策的方法。 （3）訓(xùn)練速度快。 （4）預(yù)測速度快，可以推廣到非常大的數(shù)據(jù)集，對稀疏數(shù)據(jù)也很有效。 缺點(diǎn)： （1）對于數(shù)據(jù)的準(zhǔn)確性要求高，只能對線性的問題進(jìn)行規(guī)劃約束，而且計算量大。 1.3 相關(guān)問題 運(yùn)輸問題(產(chǎn)銷平衡)、指派問題（匈牙利算法）、對偶理論與靈敏度分析、投資的收益和風(fēng)險。 2、整數(shù)規(guī)劃 規(guī)劃中的變量（部分或全部）限制為整數(shù)時，稱為整數(shù)規(guī)劃。若在線性規(guī)劃模型中，變量限制為整數(shù)，則稱為整數(shù)線性規(guī)劃。目前所流行的求解整數(shù)規(guī)劃的方法，往往只適用于整數(shù)線性規(guī)劃。目前還沒有一種方法能有效地求解一切整數(shù)規(guī)劃。 2.1 用北太天元求解線性混合整數(shù)規(guī)劃問題 可在北太天元內(nèi)調(diào)用優(yōu)化工具箱optimization，使用intlinprog等相關(guān)函數(shù)求解線性混合整數(shù)規(guī)劃問題。 2.2 整數(shù)規(guī)劃的分類 如不加特殊說明，一般指整數(shù)線性規(guī)劃。對于整數(shù)線性規(guī)劃模型大致可分為兩類： （1）變量全限制為整數(shù)時，稱純（完全）整數(shù)規(guī)劃。 （2）變量部分限制為整數(shù)的，稱混合整數(shù)規(guī)劃。 2.3 整數(shù)規(guī)劃特點(diǎn) 原線性規(guī)劃有最優(yōu)解，當(dāng)自變量限制為整數(shù)后，其整數(shù)規(guī)劃解出現(xiàn)下述情況： （1）原線性規(guī)劃最優(yōu)解全是整數(shù)，則整數(shù)規(guī)劃最優(yōu)解與線性規(guī)劃最優(yōu)解一致。 （2）整數(shù)規(guī)劃無可行解。 （3）有可行解（當(dāng)然就存在最優(yōu)解），但最優(yōu)解值變差。 整數(shù)規(guī)劃最優(yōu)解不能按照實數(shù)最優(yōu)解簡單取整而獲得。 2.4 求解方法分類 （1）分枝定界法—可求純或混合整數(shù)線性規(guī)劃。 （2）割平面法—可求純或混合整數(shù)線性規(guī)劃。 （3）隱枚舉法—求解“0-1”整數(shù)規(guī)劃：過濾隱枚舉法；分枝隱枚舉法。 （4）匈牙利法—解決指派問題（“0-1”規(guī)劃特殊情形）。 （5）蒙特卡洛法—求解各種類型規(guī)劃。 3、非線性規(guī)劃 如果目標(biāo)函數(shù)或約束條件中包含非線性函數(shù)，就稱這種規(guī)劃問題為非線性規(guī)劃問題。一般說來，解非線性規(guī)劃要比解線性規(guī)劃問題困難得多。而且，也不象線性規(guī)劃有單純形法這一通用方法，非線性規(guī)劃目前還沒有適于各種問題的一般算法，各個方法都有自己特定的適用范圍。 3.1 線性規(guī)劃與非線性規(guī)劃的區(qū)別 如果線性規(guī)劃的最優(yōu)解存在，其最優(yōu)解只能在其可行域的邊界上達(dá)到（特別是可行域的頂點(diǎn)上達(dá)到）；而非線性規(guī)劃的最優(yōu)解（如果最優(yōu)解存在）則可能在其可行域的任意一點(diǎn)達(dá)到。 3.2 相關(guān)問題 無約束問題（一維搜索方法、二次插值法、無約束極值問題的解法）、約束極值問題（二次規(guī)劃、罰函數(shù)法）、飛行管理問題 4、動態(tài)規(guī)劃 動態(tài)規(guī)劃（dynamic programming）是運(yùn)籌學(xué)的一個分支，是求解決策過程（decisionprocess）最優(yōu)化的數(shù)學(xué)方法。例如最短路線、庫存管理、資源分配、設(shè)備更新、排序、裝載等問題，用動態(tài)規(guī)劃方法比用其它方法求解更為方便。 雖然動態(tài)規(guī)劃主要用于求解以時間劃分階段的動態(tài)過程的優(yōu)化問題，但是一些與時間無關(guān)的靜態(tài)規(guī)劃（如線性規(guī)劃、非線性規(guī)劃），只要人為地引進(jìn)時間因素，把它視為多階段決策過程，也可以用動態(tài)規(guī)劃方法方便地求解。應(yīng)指出，動態(tài)規(guī)劃是求解某類問題的一種方法，是考察問題的一種途徑，而不是一種特殊算法（如線性規(guī)劃是一種算法）。因而，它不象線性規(guī)劃那樣有一個標(biāo)準(zhǔn)的數(shù)學(xué)表達(dá)式和明確定義的一組規(guī)則，而必須對具體問題進(jìn)行具體分析處理。因此，在學(xué)習(xí)時，除了要對基本概念和方法正確理解外，應(yīng)以豐富的想象力去建立模型，用創(chuàng)造性的技巧去求解。 5、多目標(biāo)規(guī)劃 多目標(biāo)規(guī)劃已經(jīng)應(yīng)用到科學(xué)的許多領(lǐng)域，包括工程、經(jīng)濟(jì)和物流，在兩個或更多沖突的目標(biāo)之間存在取舍時，需要采取最優(yōu)決策。 解決多目標(biāo)規(guī)劃問題的方法： （1）將多目標(biāo)化為單目標(biāo) (給多個目標(biāo)賦予權(quán)重) （2）保持多目標(biāo)不變，根據(jù)自己的偏好選擇解 實際問題中，目標(biāo)函數(shù)相互沖突是很常見的，例如購買汽車時，要求花費(fèi)少且舒適度高或者要求性能好油耗低，這種問題并沒有絕對最優(yōu)解（因為并沒有確定多個目標(biāo)的權(quán)值），但是我們可以根據(jù)自己的需要選擇一個相對好的（達(dá)到我們想要的最佳平衡）。為了尋求這種“最佳平衡”，可以采用算法帕累托最優(yōu)（Pareto optimal）。 以上部分內(nèi)容引用公眾號“科研交流”，希望對大家有幫助，覺得有用就點(diǎn)個贊吧。小助手會不定期更新數(shù)學(xué)建模干貨，可以多多關(guān)注喲。
線性調(diào)頻信號是一種頻譜擴(kuò)展調(diào)制技術(shù)，在雷達(dá)等領(lǐng)域具有廣泛應(yīng)用，信號頻率是一個關(guān)于時間t的線性函數(shù)。線性調(diào)頻信號時域表達(dá)式如下：其中，T為采樣周期，K為線性調(diào)頻率，K=B/T，B為信號帶寬?；诒碧煸抡婢€性調(diào)頻信號如下：北太天元代碼如下：
clear
clc
close all
clf
tao=1e-5;%脈沖寬度
B=20e6;%帶寬20MHz
fs=2*B;%采樣頻率
Ts=1/fs;
K=B/tao;%調(diào)頻斜率
N=floor(tao/Ts);
% 線性調(diào)頻信號
t=linspace(-tao/2,tao/2,N);
st=exp(j*pi*K*t.*t);%線性調(diào)頻信號
figure(1)sst = real(st);
plot(t*1e5,sst);
title('線性調(diào)頻信號時域?qū)嵅?#39;);
f=linspace(-fs/2,fs/2,N);
load_plugin("fft")
sf=abs(fft(st));%線性調(diào)頻信號頻域信號
figure(2)
ssf = [sf(N/2+1:end),sf(1:N/2)];
plot(f*1e5,ssf);
title('線性調(diào)頻信號頻譜幅值');
figure(3)
plot(angle(st))
title('線性調(diào)頻信號相位');
17.1 原理 完整思想請看我前面寫的路徑規(guī)劃（十三）基于搜索的路徑規(guī)劃算法-前言,，和其他的基于搜索的路徑規(guī)劃算法的區(qū)別僅在于啟發(fā)式函數(shù)的不同. 雙向A*則稍微復(fù)雜些，但可以簡單理解為起始節(jié)點(diǎn)和終點(diǎn)同時將對方視為目標(biāo)節(jié)點(diǎn)，并按照A*的啟發(fā)式函數(shù)，相向生長，當(dāng)兩者相遇時，則停止迭代，并分別往回追溯自己的父節(jié)點(diǎn)即可得到路徑。17.2 程序示例
16.1 原理 完整思想請看我前面寫的路徑規(guī)劃（十三）基于搜索的路徑規(guī)劃算法-前言，，和其他的基于搜索的路徑規(guī)劃算法的區(qū)別僅在于啟發(fā)式函數(shù)的不同 A*則是結(jié)合了Best-first Searching和Dijkstra，它將當(dāng)前節(jié)點(diǎn)到初始節(jié)點(diǎn)和到目標(biāo)節(jié)點(diǎn)的距離之和作為啟發(fā)式函數(shù)。16.2 程序示例16.3 參考A Formal Basis for the heuristic Determination of Minimum Cost Paths
15.1 原理完整思想請看我前面寫的路徑規(guī)劃（十三）基于搜索的路徑規(guī)劃算法-前言，和其他的基于搜索的路徑規(guī)劃算法的區(qū)別僅在于啟發(fā)式函數(shù)的不同Dijkstra則和Best-first-searching相反，它不是將到目標(biāo)節(jié)點(diǎn)的距離作為啟發(fā)式函數(shù)，而是將到起始節(jié)點(diǎn)的距離作為啟發(fā)式函數(shù)。15.2 程序示例
14.1 原理這里的Best-first-searching和數(shù)據(jù)結(jié)構(gòu)里學(xué)的圖搜索算法BFS（廣度優(yōu)先搜索）不是一個東西。完整思想請看我前面寫的路徑規(guī)劃（十三）基于搜索的路徑規(guī)劃算法-前言下面說說Best-first-searching的核心思想：Best-first Searching的啟發(fā)式函數(shù)f(x)=dist(x,x_goal)，即Best-first Searching每一步都在預(yù)選集合中尋找距離目標(biāo)節(jié)點(diǎn)最近的的那個節(jié)點(diǎn)。這里的dist(x,y)，如果節(jié)點(diǎn)x,y無法通過碰撞檢測，則為inf，如果能通過碰撞檢測，可以直接用歐幾里得距離代替。14.2 程序示例14.3 參考https://blog.csdn.net/potato_uncle/article/details/109124362?ops_request_misc=&request_id=&biz_id=102&utm_term=best%20first%20search&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-1-109124362.nonecase&spm=1018.2226.3001.4187
基于搜索的路徑規(guī)劃算法基本都是一個套路，它們都是根據(jù)啟發(fā)函數(shù)重備用節(jié)點(diǎn)的集合中來尋找下一個節(jié)點(diǎn)，不同的啟發(fā)函數(shù)也就有不同的搜索類算法。搜索類算法是離散化的算法，體現(xiàn)在整個圖的區(qū)域是由有限個小方塊區(qū)域組成的。我們暫且把這些小方塊區(qū)域稱為“節(jié)點(diǎn)”。因此，整個區(qū)域被有限個節(jié)點(diǎn)填充，且每個節(jié)點(diǎn)的鄰居節(jié)點(diǎn)為有限個。設(shè)置兩個集合OPEN,CLOSE，OPEN初始狀態(tài)設(shè)為{x_init}，CLOSE 初始狀態(tài)設(shè)為空集。依據(jù)不同的啟發(fā)式函數(shù)，從open集中選擇一個點(diǎn)加入到close集中，然后拓展open集，如上圖，右下角的某個點(diǎn)被某種啟發(fā)式函數(shù)選中，加入到close集中，并相繼拓展open集下面介紹下搜索類算法的前進(jìn)過程：當(dāng)上述偽碼退出循環(huán)后，沿著x_goal的父節(jié)點(diǎn)往前回溯極為路徑各搜索類算法的區(qū)別在于第三行啟發(fā)函數(shù)的類型的不同，導(dǎo)致連接的節(jié)點(diǎn)不同。
幾種RRT對比如下：幾種RRT對比視圖mp4 RRT及其變種都是依托于采樣+在樹結(jié)構(gòu)上加減枝的形式進(jìn)行路徑規(guī)劃的，具有全局收斂特性，但是效率穩(wěn)定性不高。不過可以針對性地對其主要函數(shù)進(jìn)行優(yōu)化進(jìn)行效率的改進(jìn)：優(yōu)化采樣，優(yōu)化樹結(jié)構(gòu)等。一種加速RRT的思路就是，從起始點(diǎn)和目標(biāo)節(jié)點(diǎn)同時生長RRT樹，這就是connected_RRT。此外，針對變化的環(huán)境，還有extend_RRT和Dynamic_RRT。 RRT*是一種趨近于最優(yōu)路徑的方案，它通過重布線來實現(xiàn)這一目的，它在理論上能達(dá)到最優(yōu)解，但它全局隨機(jī)撒點(diǎn)的特性導(dǎo)致它在遠(yuǎn)離目標(biāo)路徑的地方做了過多的生長。 為了集中優(yōu)化資源，RRT*-smart應(yīng)運(yùn)而生，它比較在乎路徑和障礙物的拐點(diǎn)的附近的優(yōu)化，它通過路徑優(yōu)化步驟判斷出路徑和障礙物的拐點(diǎn)，并在拐點(diǎn)的鄰域內(nèi)投入更多的資源（即撒更多的點(diǎn)），以實現(xiàn)集中優(yōu)化資源。 但RRT*-smart依然浪費(fèi)了太多的隨機(jī)點(diǎn)在遠(yuǎn)離目標(biāo)路徑的區(qū)域，那什么才叫不遠(yuǎn)離目標(biāo)路徑的區(qū)域呢？informed RRT*則解決了這一問題，它利用初始路徑的長度，起始點(diǎn)和目標(biāo)點(diǎn)，畫出了一個橢圓，informed RRT*認(rèn)為，這個橢圓區(qū)域就是不遠(yuǎn)離目標(biāo)路徑的區(qū)域，生成這個橢圓后，后續(xù)的隨機(jī)撒點(diǎn)只灑在這個橢圓區(qū)域內(nèi)，當(dāng)更優(yōu)的路徑被發(fā)現(xiàn)，則根據(jù)這個新路徑的長度，縮小橢圓，進(jìn)一步在有效區(qū)域集中撒點(diǎn)資源，以實現(xiàn)加速。 然而，RRT*類的算法是總會面臨一個問題，那就是重布線，這個令RRT*能夠逼近最優(yōu)解的創(chuàng)新恰恰成為了它慢的原因。 于是，另一種思路被提出，那就是提前給定隨機(jī)點(diǎn)，然后通過啟發(fā)式函數(shù)來連接這些點(diǎn)以生長路徑，這就是FMT*，F(xiàn)MT*專門針對解決高維構(gòu)型空間中的復(fù)雜運(yùn)動規(guī)劃問題，在預(yù)先確定的采樣點(diǎn)數(shù)量上執(zhí)行前向動態(tài)規(guī)劃遞歸，并相應(yīng)地通過在代價到達(dá)空間中穩(wěn)步向外移動生成路徑樹。FMT*能很快的找到一條路徑，但是當(dāng)我們想對這條路徑進(jìn)行優(yōu)化時，只有通過加密隨機(jī)采樣點(diǎn)的方式，然而，F(xiàn)MT*是一種單批算法，面對新的采樣點(diǎn)分布時，它只能重新開始計算。 為了融合informed RRT*在有效區(qū)域集中隨機(jī)點(diǎn)的特點(diǎn)和FMT*快速生長的特點(diǎn)，就誕生了BIT*。它能夠在橢圓區(qū)域內(nèi)分批撒點(diǎn)，實現(xiàn)快速生長的同時，還能自我優(yōu)化。參考https://www.youtube.com/watch?v=TQIoCC48gp4
11.1 原理 簡單來說，BIT*是結(jié)合了Informed RRT*和FMT*的優(yōu)點(diǎn)的一種算法?；仡櫼幌?，Informed RRT*是對RRT*的一種優(yōu)化，在RRT*生成一個初始路徑后，則以初始路徑的長度，起始點(diǎn)和目標(biāo)點(diǎn)為焦點(diǎn)，畫一個橢圓，Informed RRT*在后續(xù)隨機(jī)采點(diǎn)時，只取落在這個橢圓內(nèi)的點(diǎn)，一次采一個點(diǎn)，重復(fù)lm次。FMT*則與RRT那一套不同，它不是邊采點(diǎn)，邊生長樹，而是一次性提前在整個區(qū)域（不包含障礙物區(qū)域）內(nèi)采lm個點(diǎn)，只重復(fù)一次。 下面我們來說說，Informed RRT*和優(yōu)缺點(diǎn)FMT*，然后就知道為什么要引出BIT*了。 先說FMT*，F(xiàn)MT*的優(yōu)點(diǎn)是從起始位置開始構(gòu)建，沒有重布線過程，因此節(jié)約時間，適用于復(fù)雜的障礙物環(huán)境。但是FMT*的缺點(diǎn)是，它只有1批，F(xiàn)MT*路徑的精度完全取決于當(dāng)前批撒點(diǎn)的密度，當(dāng)你想要提升精度時，只能重新開始一批，重新更密集的撒點(diǎn)，然后重新開始規(guī)劃。 再說Informed RRT*，Informed RRT*的優(yōu)點(diǎn)恰好彌補(bǔ)了FMT*的缺點(diǎn)，想要提升精度，只需撒更多的點(diǎn)就好了，而Informed RRT*的撒點(diǎn)過程時一直在進(jìn)行的，它一批只撒一個點(diǎn)，重復(fù)很多批，開始新的批的時候之前的信息不會被拋棄，只要Informed RRT*一直撒點(diǎn)，就可以達(dá)到任意精度。但是Informed RRT*的缺點(diǎn)也顯而易見，它需要重布線，計算效率低。 所以自然就想到，能不能利用FMT*的優(yōu)點(diǎn)，提前撒好點(diǎn)，不用重布線，提升計算效率，又能多批進(jìn)行，以不斷提升精度？當(dāng)然能，這就是BIT*算法 BIT*的過程總結(jié)為下圖：11.2 偽碼11.3 參考1、Batch Informed Trees (BIT*): Informed Asymptotically Optimal Anytime Search2、Batch Informed Trees (BIT*): Sampling-based Optimal Planning via the Heuristically Guided Search of Implicit Random Geometric Graphs
10.1 原理 在RRT中，當(dāng)初始路徑已經(jīng)生成之后，如果重點(diǎn)在初始路徑周圍進(jìn)行采樣的話，可以明顯提高路徑優(yōu)化效率。Informed RRT就是進(jìn)一步優(yōu)化了采樣函數(shù)，采樣的方式是以起點(diǎn)和終點(diǎn)為焦點(diǎn)構(gòu)建橢圓形采樣區(qū)域。 回顧一下RRT*-smart，因為在某區(qū)域撒點(diǎn)越多，該區(qū)域的優(yōu)化效果越好，而單純的RRT*是在全域內(nèi)隨機(jī)撒點(diǎn)，優(yōu)化效果沒有得以集中，RRT*-smart認(rèn)為經(jīng)過路徑優(yōu)化后的路徑的拐點(diǎn)在障礙物的附近，它認(rèn)為這個拐點(diǎn)的附近需要著重優(yōu)化，所以RRT*-smart在進(jìn)一步撒點(diǎn)的過程中，將一些隨機(jī)點(diǎn)偏袒的撒在這個拐點(diǎn)的附近鄰域。 這里的informed RRT*也是這樣認(rèn)為，它認(rèn)為單純的RRT*在整個區(qū)域內(nèi)隨機(jī)撒點(diǎn)，優(yōu)化效果太過分散，如果我能知道我最終優(yōu)化的路徑在哪一塊區(qū)域，那我就只在這一區(qū)域內(nèi)撒點(diǎn)不就好了嗎？informed RRT*就是這樣做的。注意： informed RRT*是在RRT*算法給出一條初始路徑后，對這個初始路徑繼續(xù)優(yōu)化的步驟才起作用的，它對于這個初始路徑的生成沒有幫助。10.2 思路 根據(jù)高中數(shù)學(xué)知識可以知道，在橢圓上的點(diǎn)到橢圓兩焦點(diǎn)的距離之和相同，橢圓外的點(diǎn)的距離到兩焦點(diǎn)的距離之和大于橢圓上的點(diǎn)到兩焦點(diǎn)的距離之和，橢圓內(nèi)的點(diǎn)反之。 回顧一下RRT*的搜索圖，根據(jù)上面這個知識點(diǎn)可以發(fā)現(xiàn)，其實RRT在已經(jīng)得到一條可行路徑之后，可以將采樣空間收縮到一個橢圓形區(qū)域中，區(qū)域之外的點(diǎn)對于縮短規(guī)劃出的路徑長度并沒有實際價值。 informed RRT就是的主要思想就是上面這種思想，在獲取可行路徑之后，將采樣空間限制在一個橢圓形區(qū)域中，并且隨著路徑長度的不斷縮短，逐漸縮小該橢圓形區(qū)域。這個思想其實在以前就有，但是提出informed RRT的論文中提出了對這個橢圓形區(qū)域直接采樣的方法。 可能有人會直接想，這里只不過是縮小了采樣空間，并不會明顯改進(jìn)算法。但是實際上，當(dāng)拓展到高維空間時，效率的提升是巨大的。那么，如何表達(dá)這個橢圓呢？下面介紹橢圓采樣區(qū)域的表達(dá)方式方法1：先在標(biāo)準(zhǔn)橢圓的方程中采樣，再將采樣點(diǎn)旋轉(zhuǎn)平移到實際采樣區(qū)域，需要兩個矩陣：平移向量、旋轉(zhuǎn)矩陣。這兩個參數(shù)只需要在初始化時計算即可轉(zhuǎn)換后的坐標(biāo)為：方法2：利用超橢圓體然后在二維平面映射這里放一段.m文件取橢圓隨機(jī)點(diǎn)的代碼（思路如方法2）：除了采樣過程外，Informed RRT*的流程和RRT*是一樣的。10.3 偽碼偽代碼中是在RRT的偽代碼基礎(chǔ)上改的，標(biāo)紅的地方是informed RRT 更改的地方。可以看出，其實主體框架上面并沒有太多更改，實際上也是，主要的更改都在第七行，也就是采樣這一步。這是采樣這一步的偽代碼。informed RRT將目前已經(jīng)搜索到的最短路徑作為cbest，起點(diǎn)和終點(diǎn)之間的距離作為cmin，以此構(gòu)建橢圓。當(dāng)還沒有規(guī)劃結(jié)果時，cbest為inf，也就是和經(jīng)典RRT沒有區(qū)別。10.4 程序示例程序在尋找初始路徑的過程和普通RRT*一樣，在全局域中隨機(jī)撒點(diǎn)，迭代到1282次時首次找到初始路徑，然后我們以起始點(diǎn)和目標(biāo)點(diǎn)為焦點(diǎn)，初始路徑的長度為點(diǎn)到兩焦點(diǎn)的距離之和，畫出一個橢圓：我們隨后的隨機(jī)點(diǎn)的選取范圍不再是全局域了，新采的樣本點(diǎn)被限制在這個橢圓中，下圖中的圓圈代表迭代1283-2509次的隨機(jī)點(diǎn)的分布，可見，新的隨機(jī)點(diǎn)全部被限制在橢圓中：當(dāng)?shù)?510次時，新的總長度更短的路徑被找到，，隨后，我們以起始點(diǎn)和目標(biāo)點(diǎn)為焦點(diǎn)，以這個新的路徑的長度為到兩焦點(diǎn)的距離，畫出一個比之前更小的橢圓：同樣的，迭代次數(shù)為2510-2865次的循環(huán)中的新的隨機(jī)點(diǎn)被限制在這個新的更小的橢圓中，使隨機(jī)點(diǎn)資源進(jìn)一步集中：當(dāng)?shù)?866次時，找到一個路徑更短的路徑：10.5 參考Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic
9.1 原理 FMT*算法專門針對解決高維構(gòu)型空間中的復(fù)雜運(yùn)動規(guī)劃問題，它是為高密度障礙物的環(huán)境構(gòu)建的算法。該算法被證明是漸近最優(yōu)的，并且比同類型算法（RRT*）更快收斂到最優(yōu)解。FMT*算法在預(yù)先確定的概率繪制的樣本數(shù)量上執(zhí)行“惰性”動態(tài)規(guī)劃遞歸，以生長路徑樹，該路徑樹在成本到達(dá)空間中穩(wěn)定地向外移動。 FMT*的最終產(chǎn)物是一棵樹，它在連續(xù)空間中獲取一批樣本，然后它能在圖中使用惰性的動態(tài)編程搜索該樣本集合，并以此找到路徑，這也是一個漸進(jìn)最優(yōu)的解決方案，F(xiàn)MT*相比于RRT*的加速效果優(yōu)勢在高維和碰撞檢查很昂貴的情況下尤其突出。很棒的一點(diǎn)是，F(xiàn)MT*是從起始位置開始構(gòu)建，而不是像RRT*是在空間的任意位置采點(diǎn)，因為這可能會得到非常遠(yuǎn)的點(diǎn)或非常近的點(diǎn)，這有什么好處呢？這意味著你不必在樹中回溯以進(jìn)行重布線，因為這在計算上效率低下。FMT*比RRT*更好，因為它創(chuàng)建的連接接近最佳，沒有重布線。 FMT*算法在預(yù)先確定的采樣點(diǎn)數(shù)量上執(zhí)行前向動態(tài)規(guī)劃遞歸，并相應(yīng)地通過在代價到達(dá)空間中穩(wěn)步向外移動生成路徑樹。FMT*執(zhí)行動態(tài)規(guī)劃遞歸，其特點(diǎn)有三個關(guān)鍵特征:·它是為磁盤連接圖量身定制的，其中兩個樣本的距離低于給定的界限（稱為連接半徑）則這兩個樣本被認(rèn)為是鄰居，因此是可連接的?！に瑫r執(zhí)行圖構(gòu)造和圖搜索?！榱嗽u估動態(tài)規(guī)劃遞歸中的即時成本，算法“懶惰地”忽略了障礙物的存在，每當(dāng)與新樣本的局部最優(yōu)(假設(shè)沒有障礙物)連接與障礙物相交時，該樣本就會簡單地跳過并留待以后，而不是在鄰域中尋找其他連接。注意：FMT*的樣本點(diǎn)是提前生成好的，然后把這些點(diǎn)固定，再利用這些固定好的點(diǎn)來生成行進(jìn)樹，注意區(qū)別于RRT*那一套，RRT*是生成隨機(jī)點(diǎn)的同時，生成行進(jìn)樹。9.2 算法思路上圖（a）是RRT*添加新節(jié)點(diǎn)的某一步，上圖（b）（c）是FMT*添加新節(jié)點(diǎn)的某一步。 先看RRT*，節(jié)點(diǎn)9是新考慮的節(jié)點(diǎn)，它在第一次重布線時，需要從它的所有鄰居節(jié)點(diǎn)中找出一個父節(jié)點(diǎn)，使得節(jié)點(diǎn)9到達(dá)起始節(jié)點(diǎn)的cost最小，因此節(jié)點(diǎn)9需要計算它到4、5、6、8號節(jié)點(diǎn)的距離并同時進(jìn)行碰撞檢測，因此，第一次重布線過程就要求待加入的節(jié)點(diǎn)對它的所有鄰居節(jié)點(diǎn)進(jìn)行一次碰撞檢測，第二次重布線過程也需要計算距離和碰撞檢測，但這在第一次重布線過程中做過了，可以記錄先來直接利用，因此第二次重布線過程碰撞檢測這一步可以不用重復(fù)進(jìn)行，因此，總的來說，RRT*每新加入一個節(jié)點(diǎn)，該節(jié)點(diǎn)需要對它的所有鄰居節(jié)點(diǎn)進(jìn)行一次碰撞檢測。 再看FMT*，上圖（b）（c）中的x就是新考慮的節(jié)點(diǎn)，在圖（b）中，x需計算它到集合V_open中所有的鄰居節(jié)點(diǎn)的cost，但不需要進(jìn)行碰撞檢測，從中選擇一個能使它到達(dá)初始節(jié)點(diǎn)總cost最小的節(jié)點(diǎn)作為它的父節(jié)點(diǎn)，然后，對它和這個父節(jié)點(diǎn)的連線進(jìn)行碰撞檢測，如果能通過碰撞檢測，則加入x，若不能，則下一個x，因此，總的來說，F(xiàn)MT*每新加入一個節(jié)點(diǎn)，永遠(yuǎn)只需要進(jìn)行一次碰撞檢測。FMT*比RRT*每新加入一個節(jié)點(diǎn)需要進(jìn)行的碰撞檢測次數(shù)少得多，而且FMT*也是漸進(jìn)最優(yōu)的，這就是FMT*相比于RRT*的優(yōu)勢所在。9.3 偽碼9.4 程序示例下圖中的 圓圈代表提前采好的隨機(jī)點(diǎn)9.5 參考Fast Marching Tree: a Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions?
8.1 原理 最初，RRT*-Smart 像 RRT* 一樣隨機(jī)搜索狀態(tài)空間。類似地，找到第一條路徑就像 RRT* 會嘗試通過配置空間中的隨機(jī)采樣來找到路徑一樣。一旦找到第一條路徑，它就會通過互連直接可見的節(jié)點(diǎn)來優(yōu)化它。此優(yōu)化路徑產(chǎn)生用于智能采樣的偏置點(diǎn)。在這些偏置點(diǎn)，采樣以規(guī)則的間隔進(jìn)行 隨著算法的進(jìn)展和路徑的不斷優(yōu)化，此過程將繼續(xù)進(jìn)行。每當(dāng)找到更短的路徑時，偏差就會轉(zhuǎn)向新路徑。 RRT*是一邊生長一邊優(yōu)化的，RRT*的重心在于找到最優(yōu)路徑。RRT*樹生長到能連接起點(diǎn)和終點(diǎn)后，這就已經(jīng)有一條初始路徑了。 這顆RRT*樹可以繼續(xù)生長，越生長可以得到的路徑相比初始路徑會越優(yōu)。然而，這個繼續(xù)生長的過程對于RRT*而言效率非常低，由此衍生出RRT*-smart算法，專門解決這一問題。 注意到，RRT*-smart是在RRT*算法已生成初始路徑后，在此基礎(chǔ)上，想對初始路徑繼續(xù)優(yōu)化的步驟才起作用，所以RRT*-smart對于生成初始路徑并沒有加速幫助。 RRT*-smart的優(yōu)勢在于：它專注于提升路徑接近障礙物拐點(diǎn)處的優(yōu)化速度。RRT*-smart算法的思路是這樣的：在原始RRT*算法的基礎(chǔ)上加了兩步：①路徑優(yōu)化路徑優(yōu)化的本質(zhì)是利用三角形兩邊之和大于第三邊 假設(shè)RRT*生成的初始路徑長這樣 具體操作如下： 一旦RRT*給出了一條初始路徑，將初始路徑中彼此可見的節(jié)點(diǎn)直接相連。迭代過程從xgoal開始，向xinit檢查與每個節(jié)點(diǎn)的連續(xù)父節(jié)點(diǎn)的直接連接，直到無沖突條件失敗。下圖給出一個示例。 信標(biāo)（Z_beacons）：經(jīng)過路徑優(yōu)化后的路徑中除了起點(diǎn)和終點(diǎn)之外的節(jié)點(diǎn)，標(biāo)記為信標(biāo)（Z_beacons），如上圖中的綠點(diǎn)。②智能采樣在RRT*算法中，在生成初始路徑后，在此RRT*樹的基礎(chǔ)上繼續(xù)采點(diǎn)，采點(diǎn)越多，路徑優(yōu)化效果越好，但此采點(diǎn)，是完全隨機(jī)的采點(diǎn)，因此效率低下，RRT*-smart則不是完全隨機(jī)的采點(diǎn)。在RRT*-smart算法中，利用了這樣一種思想：智能采樣背后的想法是通過生成盡可能靠近障礙物頂點(diǎn)的節(jié)點(diǎn)來接近最優(yōu)性。在基于采樣的RRT*-smart中，路徑僅沿著靠近障礙物拐點(diǎn)的外圍進(jìn)行優(yōu)化，解決的辦法就是：在障礙物拐點(diǎn)的外圍多多采點(diǎn)。如下圖所示： 問：那么RRT*-smart如何實現(xiàn)在障礙物拐點(diǎn)的外圍多多采點(diǎn)呢？答：利用①路徑優(yōu)化過程中給出的信標(biāo)（Z_beacons）。一旦找到初始路徑，智能采樣就會開始，在以Z beacons為中心的半徑為R beacons的球中直接生成一定數(shù)量的樣本。采樣偏向于這些信標(biāo)，因為它們提供了有關(guān)障礙物頂點(diǎn)（或圓形障礙物的外圍）位置的有用線索。因此，這些信標(biāo)需要被最大節(jié)點(diǎn)包圍，以優(yōu)化這些轉(zhuǎn)彎處的路徑。與 RRT* 相比，這一特征迫使所提出的算法以更少的迭代次數(shù)達(dá)到最優(yōu)解。注意：信標(biāo)（Z_beacons）是需要隨著優(yōu)化路徑的更新而更新的。即當(dāng)z_goal.cost變小時，說明路徑得到了優(yōu)化，那么就要啟用之前①路徑優(yōu)化算法來重新確定新的z_beacons。 8.2 偽碼 8.3 程序示例下圖中的線段代表由RRT*生成的初始RRT樹，圓圈代表在初始RRT樹基礎(chǔ)上，繼續(xù)采點(diǎn)的分布，可見在幾個“拐點(diǎn)處”的圓形領(lǐng)域內(nèi)我們有額外的采點(diǎn)以加強(qiáng)在這部分的采點(diǎn) 路徑優(yōu)化后確定出拐點(diǎn) 經(jīng)過路徑優(yōu)化后的路徑： 8.4 參考1、RRT*-SMART: A Rapid Convergence Implementation of RRT*2、Rapid convergence implementation of RRT* towards optimal solution
7.1 原理 RRT*是一種基于采樣的最優(yōu)化路徑規(guī)劃方式，與RRT的區(qū)別是，RRT盡量使新節(jié)點(diǎn)以及其周圍的節(jié)點(diǎn)到起點(diǎn)的cost(可以是路徑或者時間等目標(biāo)函數(shù))最短，而不是僅僅尋找離它近的節(jié)點(diǎn)，而且在找到路徑后不會停止，而是繼續(xù)進(jìn)行采樣來優(yōu)化得到的路徑。 盡管RRT算法是一個相對高效率，同時可以較好的處理帶有非完整約束的路徑規(guī)劃問題的算法，并且在很多方面有很大的優(yōu)勢，但是RRT算法并不能保證所得出的可行路徑是相對優(yōu)化的。因此許多關(guān)于RRT算法的改進(jìn)也致力于解決路徑優(yōu)化的問題，RRT*算法就是其中一個。RRT*算法的主要特征是能快速的找出初始路徑，之后隨著采樣點(diǎn)的增加，不斷地進(jìn)行優(yōu)化直到找到目標(biāo)點(diǎn)或者達(dá)到設(shè)定的最大循環(huán)次數(shù)。RRT*算法是漸進(jìn)優(yōu)化的，也就是隨著迭代次數(shù)的增加，得出的路徑是越來越優(yōu)化的，而且永遠(yuǎn)不可能在有限的時間中得出最優(yōu)的路徑。因此換句話說，要想得出相對滿意的優(yōu)化路徑，是需要一定的運(yùn)算時間的。所以RRT*算法的收斂時間是一個比較突出的研究問題。但不可否認(rèn)的是，RRT*算法計算出的路徑的代價相比RRT來說減小了不少。RRT*算法與RRT算法的區(qū)別主要在于兩個針對新節(jié)點(diǎn) xnew 的重計算過程，分別為：·重新為 xnew 選擇父節(jié)點(diǎn)的過程·重布線隨機(jī)樹的過程7.1.1 重新選擇父節(jié)點(diǎn)過程 在新產(chǎn)生的節(jié)點(diǎn) xnew 附近以定義的半徑范圍內(nèi)尋找“近鄰”，作為替換 xnew 父節(jié)點(diǎn)的備選。依次計算“近鄰”節(jié)點(diǎn)到起點(diǎn)的路徑代價加上 xnew 到每個“近鄰”的路徑代價，具體過程見上圖。 圖(a)中表現(xiàn)的是隨機(jī)樹擴(kuò)展過程中的一個時刻，節(jié)點(diǎn)標(biāo)號表示產(chǎn)生該節(jié)點(diǎn)的順序，0節(jié)點(diǎn)是初始節(jié)點(diǎn)，9節(jié)點(diǎn)是新產(chǎn)生的節(jié)點(diǎn) xnew，6節(jié)點(diǎn)是產(chǎn)生9節(jié)點(diǎn)的 xnear，節(jié)點(diǎn)與節(jié)點(diǎn)之間連接的邊上數(shù)字代表兩個節(jié)點(diǎn)之間的歐氏距離(這里我們用歐氏距離來表示路徑代價)。 在重新找父節(jié)點(diǎn)的過程中，以9節(jié)點(diǎn) xnew 為圓心，以事先規(guī)定好的半徑，找到在這個圓的范圍內(nèi) xnew 的近鄰，也就是4，5，8節(jié)點(diǎn)。 原來的路徑0 - 4 - 6 - 9代價為10 + 5 + 1 = 16，備選的三個節(jié)點(diǎn)與 xnew 組成的路徑0 - 1 - 5 - 9，0 - 4 - 9和0 - 1 - 5 - 8 - 9代價分別為3 + 5 + 3 = 11，10 + 4 = 14和3 + 5 + 1 + 3 = 12，因此如果5節(jié)點(diǎn)作為9節(jié)點(diǎn)的新父節(jié)點(diǎn)，則路徑代價相對是最小的，因此我們把9節(jié)點(diǎn)的父節(jié)點(diǎn)由原來的節(jié)點(diǎn)4變?yōu)楣?jié)點(diǎn)5，則重新生成的隨機(jī)樹如圖(b)所示。7.1.2 重布線隨機(jī)樹過程 在為xnew 重新選擇父節(jié)點(diǎn)之后，為進(jìn)一步使得隨機(jī)樹節(jié)點(diǎn)間連接的代價盡量小，為隨機(jī)樹進(jìn)行重新布線。過程示意如上圖重布線的過程也可以被表述成：如果近鄰節(jié)點(diǎn)的父節(jié)點(diǎn)改為 xnew 可以減小路徑代價，則進(jìn)行更改。 如圖(c)，9節(jié)點(diǎn)為新生成的節(jié)點(diǎn) xnew ，近鄰節(jié)點(diǎn)分別為節(jié)點(diǎn)4 , 6 , 8 。它們父節(jié)點(diǎn)分別為節(jié)點(diǎn)0 , 4 , 5。路徑分別為0 - 4，0 - 4 - 6，0 - 1 - 5 - 8，代價分別為10，10 + 5 = 15 和3 + 5 + 1 = 9。 如果將4節(jié)點(diǎn)的父節(jié)點(diǎn)改為9節(jié)點(diǎn) xnew ，則到達(dá)節(jié)點(diǎn)4的路徑變?yōu)? - 1 - 5 - 9 - 4，代價為3 + 5 + 3 + 4 = 15 大于原來的路徑代價10，因此不改變4節(jié)點(diǎn)的父節(jié)點(diǎn)。 同理，改變了8節(jié)點(diǎn)的父節(jié)點(diǎn)，路徑代價將由原來的9變?yōu)?4，也不改變8節(jié)點(diǎn)的父節(jié)點(diǎn)。如果改變6節(jié)點(diǎn)的父節(jié)點(diǎn)為 xnew 則路徑變?yōu)? - 1 - 5 - 9 - 6，代價為3 + 5 + 3 + 1 = 12小于原來的路徑代價15，因此將6的父節(jié)點(diǎn)改為節(jié)點(diǎn)9，生成的新隨機(jī)樹如圖(d)。 重布線過程的意義在于每當(dāng)生成了新的節(jié)點(diǎn)后，是否可以通過重新布線，使得某些節(jié)點(diǎn)的路徑代價減少。如果以整體的眼光看，并不是每一個重新布線的節(jié)點(diǎn)都會出現(xiàn)在最終生成的路徑中，但在生成隨機(jī)樹的過程中，每一次的重布線都盡可能的為最終路徑代價減小創(chuàng)造機(jī)會。 RRT*算法的核心在于上述的兩個過程：重新選擇父節(jié)點(diǎn)和重布線。這兩個過程相輔相成，重新選擇父節(jié)點(diǎn)使新生成的節(jié)點(diǎn)路徑代價盡可能小，重布線使得生成新節(jié)點(diǎn)后的隨機(jī)樹減少冗余通路，減小路徑代價。7.2 偽碼7.3 程序示例7.4 參考1、Sampling-based Algorithms for Optimal Motion Planning2、https://blog.csdn.net/weixin_43795921/article/details/88557317#t03、https://zhuanlan.zhihu.com/p/51087819
6.1原理 Dynamic RRT和Extended RRT一樣，也是用來解決動態(tài)路徑規(guī)劃問題，它們的思想有一點(diǎn)是共通的，那就是不要完全放棄初始RRT生成的樹或初始路徑的信息，而是在此基礎(chǔ)上重新規(guī)劃。Dynamic RRT和Extended RRT的區(qū)別在于，Extended RRT利用的是RRT生成的初始路徑的信息，而Dynamic RRT利用的是RRT生成的初始RRT樹的信息。思路如下： 在老地圖中，用RRT算法生成了一個RRT樹，在新地圖中，原始RRT樹的節(jié)點(diǎn)信息（坐標(biāo)、父節(jié)點(diǎn)）存儲在一個節(jié)點(diǎn)集合中。在新地圖中，先檢測新地圖中比老地圖多出的障礙物，然后，以碰撞檢測為評判根據(jù)，刪除老節(jié)點(diǎn)集合中與新障礙物無法通過碰撞檢測的節(jié)點(diǎn)和邊。的到一顆修建過后的與新地形無碰撞的修剪后RRT樹，然后再在這顆修剪后的額RRT樹的基礎(chǔ)上，繼續(xù)生長這棵樹，直到這棵樹連接起點(diǎn)和終點(diǎn)，然后回溯路徑，得出新路徑。①從從初始配置到目標(biāo)配置生成的 RRT 開始（圖（a））。 ②當(dāng)配置空間發(fā)生變化時（例如通過接收新信息），將RRT中因這些變化而失效的所有部分標(biāo)記為無效（圖（b）和（c））。 ③然后我們修剪樹以去除所有這些無效部分（圖（d））。 ④此時，保證樹中剩余的所有節(jié)點(diǎn)和邊都是有效的，但樹可能不再達(dá)到目標(biāo)。最后，我們把樹長出來，直到再次達(dá)到目標(biāo)6.2 偽碼6.3 程序示例加入新的障礙物后，被該障礙物折斷的剩余的圖：在原來的樹的基礎(chǔ)上，繼續(xù)生長后的圖：6.4 參考Replanning with RRTs
5.1 原理 在現(xiàn)實世界的場景中，通常會出現(xiàn)這樣的情況:有關(guān)環(huán)境的初始可用信息是不完整的，或者環(huán)境本身是動態(tài)的。在這些情況下，當(dāng)接收到新信息時，初始解決方案可能會失效，例如通過機(jī)載傳感器。當(dāng)這種情況發(fā)生時，通常會放棄當(dāng)前的 RRT，并從零開始生長新的 RRT。這可能是一項非常耗時的操作，尤其是在規(guī)劃問題很復(fù)雜的情況下。另一方面，在確定性規(guī)劃界存在重規(guī)劃算法，當(dāng)這種變化發(fā)生時，它們能夠有效地修復(fù)之前的解決方案，而不需要從頭重新規(guī)劃。這就是通過連續(xù)域規(guī)劃路徑的問題，如果每次更新地圖，都用RRT重新規(guī)劃，效率相當(dāng)?shù)拖?。Extended_RRT則專門用于解決這種動態(tài)路徑規(guī)劃問題。 Extended_RRT的思路是這樣的：在老地圖中，由RRT算法得出的路徑，在障礙物動態(tài)變化不太大的前提下，在新地圖中，大概率也是能通過的，就算障礙物變化很大，之前的路徑也或多或少包含了當(dāng)前障礙物區(qū)域的信息，所以，在新障礙物區(qū)域中，在重新規(guī)劃路徑時，原有的初始RRT路徑的信息可以利用上，而沒有必要完完全全重零開始用RRT來規(guī)劃。 那么如何利用上初始RRT路徑的信息呢？思路如下： 在老地圖上先用RRT算法的處一條初始路徑，將這條初始路徑上的節(jié)點(diǎn)存儲在集合waypoints中，當(dāng)環(huán)境更新后，希望在新地圖上得出一條路徑，在隨機(jī)撒點(diǎn)的步驟，新的隨機(jī)點(diǎn)有概率p落在目標(biāo)節(jié)點(diǎn)處，此外，還有r的概率落在waypoints的節(jié)點(diǎn)中，剩余1-p-r的概率在目標(biāo)區(qū)域內(nèi)隨即撒點(diǎn)。這樣在重規(guī)劃時，就可以把初始路徑的信息利用進(jìn)來。完成隨機(jī)點(diǎn)選取后，剩余的碰撞檢測、樹的生長、路徑回溯步驟與RRT一致?？偨Y(jié)：Extended_RRT適用于需要反復(fù)路徑重規(guī)劃的場景中，效率比直接重新進(jìn)行RRT要高得多，和RRT的主要區(qū)別在于，在選取新的隨機(jī)點(diǎn)時，利用上了初始路徑的信息，而不是完全隨機(jī)撒點(diǎn)。5.2偽碼5.3 程序示例5.4 參考1、Real-Time Randomized Path Planning for Robot Navigation* 2、Extended RRT algorithm with dynamic N-dimensional cuboid domains3、Replanning with RRTs
原理相比于最原始的 RRT 算法的一些缺點(diǎn)，提出的一種改進(jìn)的 RRT 算法 為了加快隨機(jī)樹到達(dá)目標(biāo)點(diǎn)的速度，簡單的改進(jìn)方法是：在隨機(jī)樹每次的生長過程中，根據(jù)隨機(jī)概率（0.0 到 1.0 的隨機(jī)值 p）來選擇生長方向是目標(biāo)點(diǎn)還是隨機(jī)點(diǎn)。2001 年，LaValle在采樣策略方面引入 RRT GoalBias 與 RRT GoalZoom，RRT GoalBias 方法中，規(guī)劃器隨機(jī)采樣的同時,以一定概率向最終目標(biāo)運(yùn)動；RRTGoalZoom 方法中，規(guī)劃器分別在整個空間和目標(biāo)點(diǎn)周圍的空間進(jìn)行采樣。 和普通RRT的區(qū)別僅在于隨機(jī)撒點(diǎn)的時候有區(qū)別，這個p越大，算法越快，但對于復(fù)雜地形，可能會陷入局部極小處，反而變慢。一般取p=0.1程序示例參考Rapidly-Exploring Random Trees: A New Tool for Path Planning
2.1 原理雙樹RRT是在原本RRT的基礎(chǔ)上多加了?顆隨機(jī)探索樹，各自從起點(diǎn)和終點(diǎn)向外探索拓展，直到兩棵樹相遇時規(guī)劃算法收斂。這種改進(jìn)過的探索策略可以??提?RRT的運(yùn)?效率。 雙樹RRT中存在兩顆隨機(jī)樹，我們將其命名為A和B，A以起點(diǎn)為根節(jié)點(diǎn)，B以終點(diǎn)為根節(jié)點(diǎn)。兩顆隨機(jī)樹的拓展方式和單樹RRT的別無二致，同樣都需要經(jīng)歷隨機(jī)采樣+步?限制+碰撞檢測這三個步驟，但是不同的地?在于雙樹RRT的隨機(jī)樹是交替生長的，??說第?輪迭代中A樹向外??，第?輪便切換為B樹??，如此循環(huán)。 在每輪迭代中，隨機(jī)樹除了向外拓展之外，還會多出?個步驟，就是遍歷另一顆隨機(jī)樹中的所有節(jié)點(diǎn)，找出離NewNode最近的節(jié)點(diǎn)，用于判斷兩顆隨機(jī)樹是否相遇。 假設(shè)算法經(jīng)歷了N次迭代以后，已經(jīng)拓展出如下圖所示的兩顆隨機(jī)樹。并且在下?輪迭代中，輪到A樹進(jìn)?拓展，A樹在圖中?綠?線條表示，B樹?黃色線條表示。當(dāng)進(jìn)?本輪迭代后，算法成功拓展出A樹的新節(jié)點(diǎn)NewNode_A，此時算法將遍歷B樹中的所有節(jié)點(diǎn)，找出B樹中離NewNode_A最近的節(jié)點(diǎn)ClosestNode_B。并判斷?者是否滿?步?限制以及是否可以通過步?檢測。如下圖所示，這種情況明顯?法通過碰撞檢測。那么A樹和B樹在這?輪迭代中?法相遇，需要接著下?輪迭代。進(jìn)?下?輪迭代，這次便切換為B樹進(jìn)?拓展，假設(shè)算法拓展出的NewNode_B以及遍歷A樹后得到的ClosestNode_A如下圖所示，經(jīng)過判斷發(fā)現(xiàn)?者滿?步?限制并且通過了碰撞檢測，那么這時A樹和B樹就成功得相遇了，規(guī)劃算法收斂當(dāng)算法收斂以后，只需在兩棵樹的相遇處分別沿著?節(jié)點(diǎn)回溯便可以找出從起點(diǎn)到終點(diǎn)的有效路徑。注意：雙向RRT和RRT的區(qū)別不僅僅是在于雙向生長，雙向RRT比RRT更“貪心”，相比于RRT在生長RRT樹的時候，是每產(chǎn)生一個隨機(jī)點(diǎn)，如果能通過碰撞檢測，就往該隨機(jī)點(diǎn)的方向生長一次，然后該隨機(jī)點(diǎn)就被廢棄了，下一步想繼續(xù)生長RRT樹的話，就只能繼續(xù)生成新的隨機(jī)點(diǎn)，每個隨機(jī)點(diǎn)最多利用一次。而雙向RRT在生長RRT樹時，先先生成一個隨機(jī)點(diǎn)，然后，該樹往該隨機(jī)點(diǎn)的方向生長，直到碰到障礙物或則生長到該隨機(jī)點(diǎn)，這樣，一個隨機(jī)點(diǎn)就被多次利用，加快了速度。2.2 偽碼2.3 程序示例2.4 收斂性分析雙向RRT的收斂性分析可以應(yīng)用RRT的收斂性分析2.5 參考1、RRT-Connect: An Efficient Approach to Single-Query Path Planning2、https://www.guyuehome.com/9405
1.1 RRT算法思路我們有兩個節(jié)點(diǎn)，一個綠色的起點(diǎn)，一個黃色的終點(diǎn)對于RRT，我們做的第一件事就是將起點(diǎn)設(shè)置為隨機(jī)樹的根，那么我們就擁有了一顆只有根節(jié)點(diǎn)的樹這棵樹光禿禿的，只有根節(jié)點(diǎn)的話不但難看，而且還沒用。那么我們這時候就需要從這個根節(jié)點(diǎn)出發(fā)，向外拓展出新的葉?。拓展的方式很簡單，就是隨機(jī)采樣+步?限制+碰撞檢測。 RRT在每輪迭代中會?成?個隨機(jī)采樣點(diǎn)NewNode，如果NewNode位于自由區(qū)域，那么我們就可以遍歷隨機(jī)樹中已有的全部節(jié)點(diǎn)，找出距離NewNode最近的節(jié)點(diǎn)ClosestNode。利?距離函數(shù)dist(NewNode, ClosestNode)得到二者之間的距離，如果滿足步長限制的話，我們將接著對這兩個節(jié)點(diǎn)進(jìn)?碰撞檢測，如果不滿足步長限制的話，我們需要沿著NewNode和ClosestNode的連線?向，找出?個符合步長限制的中間點(diǎn)，用來替代NewNode。最后如果NewNode和ClosestNode通過了碰撞檢測，就意味著二者之間存在邊（edge），我們便可以將NewNode添加進(jìn)隨機(jī)樹中。首先以第一輪迭代為例，因為剛開始我們的隨機(jī)樹中只有根節(jié)點(diǎn)，所以?論NewNode位于何處，遍歷出的最近節(jié)點(diǎn)ClosestNode必然是根節(jié)點(diǎn)。 假設(shè)我們遇到下圖這種情況，雖然采樣點(diǎn)NewNode位于步?限制之內(nèi)，但是卻很不巧沒有落在自由區(qū)域，即采樣點(diǎn)落在障礙物的位置時，這個采樣點(diǎn)會被算法舍棄。假設(shè)我們的步?限制為R，也就是說對于每個ClosestNode節(jié)點(diǎn)來說，只有當(dāng)NewNode落在其半徑為R的圓的范圍內(nèi)時，這個隨機(jī)采樣點(diǎn)NewNode才有可能被直接采納。如下圖所示，該紅?隨機(jī)采樣點(diǎn)雖然位于?由區(qū)域，但是明顯在根節(jié)點(diǎn)的步?限制之外。不過這個節(jié)點(diǎn)并不會被簡單粗暴地舍棄。?是會沿著ClosestNode和NewNode的連線，找出符合步?限制的中間點(diǎn)，將這個中間點(diǎn)作為新的采樣點(diǎn)。如下圖所示，藍(lán)點(diǎn)就可以替代紅點(diǎn)作為新的采樣點(diǎn)。那么假設(shè)我們已經(jīng)通過第?輪迭代拓展出第?個葉?節(jié)點(diǎn)A，毫?疑問地A的?節(jié)點(diǎn)就是根節(jié)點(diǎn)，假設(shè)我們第?輪迭代的隨機(jī)采樣點(diǎn)NewNode為圖中的點(diǎn)B，B落在A的步?限制范圍內(nèi)，但是A，B之間由于障礙物的阻擋，?法通過碰撞檢測，于是B就會被算法舍棄。假設(shè)我們的隨機(jī)采樣點(diǎn)是下圖中的B’，明顯B’位于?由區(qū)域，滿?步?條件，并且可以通過與點(diǎn)A的碰撞檢測，那么我們就在B’和A之間添加?條邊，并且將A設(shè)置為B’的?節(jié)點(diǎn)。學(xué)過數(shù)據(jù)結(jié)構(gòu)的?伙伴?定知道，在樹結(jié)構(gòu)中每個節(jié)點(diǎn)最多只有?個?節(jié)點(diǎn)，?節(jié)點(diǎn)可以擁有多個?節(jié)點(diǎn)。在經(jīng)歷了N輪迭代后，我們已經(jīng)獲得了?顆如下圖所示的隨機(jī)樹，這時我們發(fā)現(xiàn)此時的隨機(jī)采樣點(diǎn)竟然幸運(yùn)地落在了終點(diǎn)的步?限制范圍內(nèi)，并且?者之間不存在障礙物。這時我們便可以認(rèn)為，該采樣點(diǎn)和終點(diǎn)之間存在?條邊，于是將該節(jié)點(diǎn)設(shè)為終點(diǎn)的?節(jié)點(diǎn)，并把終點(diǎn)添加進(jìn)隨機(jī)樹。此時算法就可以結(jié)束迭代了，即規(guī)劃算法收斂。當(dāng)規(guī)劃算法收斂以后，只需要從終點(diǎn)開始，沿著其?節(jié)點(diǎn)進(jìn)?回溯，就可以找 到起點(diǎn)-終點(diǎn)之間的有效路徑。那么總結(jié)?下，RRT?成的每輪迭代中都包含以下這些流程：1. ?成?個隨機(jī)采樣點(diǎn)NewNode，并判斷采樣點(diǎn)是否位于?由區(qū)域2. 遍歷隨機(jī)樹，找出距離NewNode最近的節(jié)點(diǎn)ClosestNode3. 判斷NewNode是否在ClosestNode的步?限制范圍內(nèi)，否則尋找中間點(diǎn)替代NewNode4. 判斷NewNode和ClosestNode之間是否存在障礙物，即碰撞檢測。5. 如果NewNode滿?以上所有約束條件，則將NewNode添加進(jìn)隨機(jī)樹，設(shè)置ClosestNode為NewNode的?節(jié)點(diǎn)。6. 判斷NewNode是否在終點(diǎn)的步?限制范圍內(nèi)，并對其?者做碰撞檢測。如果滿?條件則將該NewNode設(shè)為終點(diǎn)的?節(jié)點(diǎn)，并將終點(diǎn)加?隨機(jī)樹，即可結(jié)束迭代。否則繼續(xù)迭代。1.2 偽碼1.3 RRT的收斂性分析本節(jié)主要介紹了該規(guī)劃?法的理論特性。Theorem 1 如果存在長度為k的連接序列，則將 x_init 連接到 x_goal 所預(yù)期的迭代次數(shù)不超過k/p Pf. 參考S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400. 第392頁Theorem 2 Pf. 參考S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400. 第393頁Theorem 3 當(dāng)頂點(diǎn)數(shù)趨于無窮時，在 x_init 處初始化的RRT包含 x_goal 的概率將趨近于1 Pf. 參考S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400. 第394頁定理1和定理2表示了規(guī)劃器的收斂率，定理3建?了規(guī)劃器是概率完備的（即，當(dāng)?shù)鷶?shù)趨于?窮時，找到解的概率趨于1我們可以看出，由于算法在未達(dá)到收斂條件之前是在不斷進(jìn)?迭代的，所以只要在規(guī)劃的起點(diǎn)和終點(diǎn)之間是存在有效的路徑，那么只要迭代的次數(shù)夠多，那么采樣點(diǎn)就夠多，隨機(jī)樹就長得越茂密，能探索到的區(qū)域就?夠?，就必然可以找到有效的路徑。所以RRT是概率完備的。但是由于采樣點(diǎn)每次都是隨機(jī)的，所以算法并不能保證找到的路徑是最優(yōu)的路徑。因此RRT是?最優(yōu)的。1.4 程序示例1.5 參考1、S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400.2、Rapidly-Exploring Random Trees: A New Tool for Path Planning3、https://www.guyuehome.com/9405
什么是路徑規(guī)劃？ 路徑規(guī)劃（也叫運(yùn)動學(xué)規(guī)劃），任務(wù)是確定控制輸入，以驅(qū)動機(jī)器人從初始配置和速度到目標(biāo)配置和速度，同時服從基于物理的動力學(xué)模型，且能確保機(jī)器人在環(huán)境中避開障礙。說白了，就是給你一張地圖，且已知障礙物分布，以及起始點(diǎn)和目標(biāo)點(diǎn)的坐標(biāo)，希望你根據(jù)這些信息，找到一條從起點(diǎn)到終點(diǎn)的能繞開障礙物的有效路徑，如果可以，還希望這條有效路徑盡可能最優(yōu)（最短），并且希望找到這條有效路徑的時間盡可能短（算法足夠高效） 目前流行的路徑規(guī)劃分為兩大類：基于采樣的路徑規(guī)劃和基于搜索的路徑規(guī)劃。運(yùn)動規(guī)劃的狀態(tài)空間是應(yīng)用于機(jī)器人變換的集合，稱為位姿空間（configuration space），引入了 C-空間、C-空間障礙物、自由空間等一系列概念，下面介紹一些概念：位姿（configuration）機(jī)器人一個位姿指的是一組相互獨(dú)立的參數(shù)集，它能完全確定機(jī)器人上所有的點(diǎn)在工作空間 W 中的位置，這些參數(shù)用來完整描述機(jī)器人在工作空間 W 中的狀態(tài)。一個位姿通常表示為帶有位置和方向參數(shù)的一個向量(vector)，用 q 表示。自由度（degrees of freedom）機(jī)器人的自由度定義為機(jī)器人運(yùn)動過程中決定其運(yùn)動狀態(tài)的所有獨(dú)立參數(shù)的數(shù)目，即 q 的維數(shù)。位姿空間（configuration space）位姿空間是機(jī)器人所有可能位姿組成的集合，代表了機(jī)器人所有可能的運(yùn)動結(jié)果，稱為 C-空間，也可簡記為 C。距離函數(shù)（distance function）C-空間中的距離函數(shù)定義為該空間中的一個映射
公眾號
版權(quán)所有 ©北太振寰（重慶）科技有限公司 | 渝ICP備2022008375號-4 法律聲明 隱私政策